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The  objective  of  this  study  is  to  develop  the  algorithms 
necessary  to  control  the  optimum  grasp  with  multifingered  end 
effectors  for  unexpected  changes  in  the  external  conditions. 

Control  algorithms  are  developed  for  incorporating  in  real 
time  the  necessary  modification  of  the  grasp  to  insure  that 
slip  does  not  occur  due  to  unexpected  changes  in  the  applied 
forces  and  moments  beyond  the  allowable  limits  afforded  by  the 
initial  grasp. 

The  primary  emphasis  is  to  provide  the  simplest  sensor 
system  and  logic  scheme  with  programmed  rules  to  execute  the 
best  change  of  the  orientation  of  the  object  and  the  normal 
forces  necessary  to  prevent  any  impending  slip  before  it 
occurs . 


vi 


A 3 -finger  hand  with  frictional  forces  and  moments  is 
used  for  illustration.  Two  sensor  systems  are  considered  in 
structuring  the  real  time  control  of  the  grasp. 

Several  computer  simulation  examples  are  run  to  evaluate 
the  feasibility  of  the  developed  algorithms  and  the 
limitations  on  their  application. 

This  investigation  is  intended  primarily  for  application 
in  light  weight  manipulators  handling  delicate  objects  where 
compliance  rather  than  inertia  becomes  the  dominant  factor. 
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CHAPTER  1 
INTRODUCTION 

1.1  Introduction 

A gripper,  hand,  or  end-effector,  is  the  mechanism  placed 
at  the  end  of  a robot  arm,  enabling  it  to  pick  up  work  pieces 
and  hold,  manipulate,  transfer,  place  and  release  them 
accurately  at  a given  location.  Consequently , the  gripper  is 
the  most  important  mechanical  interface  between  the  robot  and 
its  environment.  Without  the  gripper',  in  many  circumstances, 
the  robot  can  not  function  effectively. 

In  spite  of  the  importance  of  the  gripper,  practical 
gripper  technology  is  relatively  under-developed  at  this  time. 
This  is  because  research  and  development 1 work  on  grippers  is 
fragmented  and  somewhat  uncoordinated.  The  results  of 
academic  research  may  not  quickly  reach  the  practicing 
engineer  who  is  charged  with  implementing  robot  design  and 
gripper  procurement  for  particular  applications. 

The  modern  robot  is  a .computerized  multipurpose  machine 
inherently  capable  of  being  reprogrammed  for  different  tasks. 
It  is  necessary  to  develop  a control  system  which  can  manage 
the  gripper  under  unexpected  disturbance  when  it  grasps  an 
obj  ect . 
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1.2  Review  of  Literature 

The  literature  related  to  the  design  and  control  of 
multifingered  hands  is  too  extensive  to  review  in  a 
comprehensive  manner.  The  following  is  a brief  description  of 
some  of  the  published  work  which  is  relevant  to  the  problem 
under  investigation. 

1.2.1  Design  for  Multi-fingered  Hands 

The  design  and  development  of  the  grasp  by  robotic 
fingers  has  been  a good  and  important  research  topic  for  the 
past  two  decades.  The  book  on  "Mechanical  Hands  Illustrated" 
edited  by  Sadamoto  Kuni  [1984]  which  includes  detailed 
description  of  large  number  of  hands  for  a wide  variety  of 
application  illustrates  the  considerable  interest  in  this 
subject.  One  of  the  earlier  researchers,  Crossley  [1975], 
designed  a three  fingered  hand  for  use  on  a remotely 
controlled  robot.  It  was  able  to  pick  up  a tool  and  draw  it 
into  a nested  grip  against  the  palm,  and  was  also  able  to  hold 
a pistol-grip  tool  such  as  an  electric  drill  and  pull  the 
trigger. 

1.2.2  Modeling  Contacts  with  Soft  Fingers 

Cutkosky  and  Wright  [1986]  used  a shearing  model  to 
describe  the  contact  friction.  They  developed  models  of 
pointed,  curved,  flat,  soft,  and  soft-curved  fingertips  and 
compared  them  in  terms  of  their  contribution  to  the  stiffness 
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and  stability  of  a simple  grasp.  They  concluded  that  there 
are  numerous  advantages  to  using  compliant  materials  for  the 
fingers  of  a robot  gripper.  The  direct  advantages  are  as 
follows : 

1.  Compliant  materials  distribute  contact  forces, 
avoiding  damage  to  the  gripped  object. 

2.  Compliant  materials  have  large  coefficients  of 
friction,  making  it  possible  to  use  lower  griping  forces. 

3 . Compliant  griping  surfaces  have  more  kinematic 
coupling  at  each  contact  with  an  object  so  that  fewer  contacts 
are  required  to  completely  restrain  an  object.  For  example, 
two  soft  fingers  can  always  restrain  an  object  in  space  but 
two  hard  fingers  can  not. 

One  of  the  important  problems  in  the  articulated  robot 
hands  is  the  control  of  the  initial  impact  force,  which  is 
produced  at  the  onset  of  contact  between  the  robot  finger  and 
the  obj  ect . 

Kahng  and  Amirouche  [1987]  presented  the  derivation  of 
the  maximum  impact  force  equation  for  a general  two-body 
collision  problem  using  the  energy  principle.  The  equation  of 
the  impact  force  utilizes  the  kinetic  energy,  the  strain 
energy  stored  in  the  elastic  bodies,  and  the  energy  lost  due 
to  structural  damping.  They  compared  the  results  with  data 
obtained  from  an  experiment. 
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Howe,  Kao,  and  Cutkosky  [1988]  investigated  finding  the 
magnitudes  of  applied  moment  and  force  which  will  cause  a 
robot  finger  to  slip  on  the  surface  of  a grasped  object.  They 
described  an  improved  model  which  includes  torsion-shear 
interaction.  Their  experimental  measurements  suggest  that  a 
simple  linear  function  of  torsion  and  shear  magnitudes  will 
adequately  predict  the  onset  of  slip  in  many  tasks. 

Sinha  and  Abel  [1989]  developed  a model  which  utilizes  a 
contact-stress  analysis  of  an  arbitrarily  shaped  object  in  a 
multifingered  grasp.  The  fingers  and  the  object  were  all 
treated  as  elastic  bodies  and  the  region  of  contact  was 
modeled  as  a deformable  surface  patch.  The  relationship 
between  the  friction  and  normal  forces  was  nonlocal  and 
nonlinear  in  nature  and  departs  from  the  Coulomb 
approximation . 

The  superior  performance  of  human  fingers  motivated 
Akella  and  Cutkosky  [1989]  to  create  soft,  anthropomorphic 
fingertips  for  dexterous  manipulation.  They  presented  an 
attempt  to  model  soft  fingertips  filled  with  powders  or 

plastic  fluids.  They  reported  sensitivity  studies  to  help  in 

# 

the  choice  of  an  appropriate  model.  The  visco-plastic  nature 
of  the  fingertip  affected  the  dynamics  of  manipulation  by 
dissipating  energy.  The  results  of  their  computer  simulation 
showed  that  soft  fingertips  can  help  stabilize  the  grasp  and 
reduce  the  demands  on  the  control  system. 
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1.2.3  Analysis  and  Optimization  of  the  Grasp 

The  analysis  and  optimization  of  the  grasp  is  a subject 
of  primary  concern  for  designing  mechanical  grippers.  One  of 

t 

the  early  investigations  related  to  this  area  was  undertaken 
by  Yuan  and  Freudenstein  [1971]  who  developed  the  concept  of 
screw  coordinates  in  terms  of  motor  algebra,  and  applied  it  to 
the  kinematics  and  static  equilibrium  of  grasping  rigid 
bodies.  They  also  developed  the  laws  of  the  composition  and 
transformation  of  screw  coordinates  and  their  application  to 
this  class  of  problems. 

Holtzman  and  McCarthy  [1985]  presented  a procedure  for 
computing  the  frictional  forces  required  to  restrain  an  object 
using  a three  fingered  hand  for  any  given  surface  normals,  the 
contact  points  and  magnitude  of  the  applied  normal  forces. 
They  used  the  screw  theory  and  point  contact  model  with 
friction  which  allows  the  fingertip  to  be  free  to  rotate  about 
the  point  of  contact . 

Fearing  [1986]  presented  a method  for  stably  grasping  two 
dimensional  polygonal  objects  considering  a dexterous  hand 
with  two  fingers.  He  developed  the  basic  constraints  on  the 
vertex  angles  of  the  object  for  a stable  grasp  using  two 
fingers . 

Kerr  and  Sanger  [1987]  introduced  6x6  stiffness 
matrices  for  computing  the  friction  forces  and  the  finger 
normal  forces  in  contact  with  any  arrangement  of  point 
contacts  having  known  frictional  properties.  They  established 
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the  relationship  between  externally  applied  loads  and  contact 
forces  in  the  presence  of  limiting  friction  which  can  be  used 
for  evaluating  the  resulting  bodily  motion,  with  or  without 
slip  at  the  contact.  They  treated  the  excess  frictional  force 
as  a disturbance  on  the  original  system,  to  find  a body  twist 
which  in  turn  will  give  a modified  set  of  local  forces.  They 
used  the  screw  theory  and  point  contact  model  with  friction 
which  allows  the  fingertip  to  be  free  to  rotate  about  the 
point  of  contact  without  frictional  moment  resistance. 

Van-Due  Nguyen  [1987]  presented  fast  and  simple 
algorithms  for  directly  constructing  stable  grasps  in  3-D  by 
developing  a simple  geometric  relation  between  the  stiffness 
of  the  grasp  and  the  spatial  configuration  of  the  virtual 
springs  at  the  contacts . He  introduced  linear  and  angular 
spring  models  in  3-D  to  construct  stiffness  matrices.  Their 
main  results  show  that  all  3-D  force-closure  grasp  can  be  made 
stable.  They  also  found  that  the  stiffness  matrix  K of  the 
grasp  is  the  sum  of  two  matrices  Ks  and  Kp  (Ks  depends  on  the 
spatial  configuration  of  the  virtual  springs.  Kp  depends  on 
whether  the  finger  stick  or  slide  on  the  straight  edges  of  the 
ob j ect ) . 

Kumar  and  Waldron  [1987]  obtained  the  multifinger 
reaction  forces  and  friction  forces  in  the  grasp  by  two 
suboptimal  procedures.  In  each  procedure,  a pseudo-inverse 
technique  was  used  to  solve  the  undetermined  linear  equations. 
The  purpose  of  this  method  is  to  reduce  computational  time  so 
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that  it  can  be  used  in  real  time  control  with  currently 
available  computer  hard-ware.  They  proposed  two  approaches. 
In  both  cases,  they  decoupled  the  sub-problems  for  finding 
forces  parallel  to  the  x-y  plane  and  then  forces  parallel  to 
the  wrench  axis.  They  also  used  the  screw  theory  and  point 
contact  model  with  friction  which  allows  the  fingertip  to  be 
free  to  rotate  about  the  point  of  contact. 

Badreldin  and  Seireg  [1988,1990]  developed  a generalized 
optimization  algorithm  for  minimizing  the  maximum  finger 
normal  forces  when  grasping  a moving  object.  They  considered 
all  frictional  forces  and  moments  for  any  multi-finger  end 
effector  when  handling  objects  of  any  shape  along  given 
trajectories  in  a given  time.  This  technique  is  particularly 
useful  in  designing  end  effectors  for  handling  delicate 
objects  by  planning  the  number  of  fingers  and  the 
corresponding  best  locations  for  the  grasp  by  the  different 
fingers . 

Demmel  and  Lafferiere  [1989]  described  a procedure  to 
compute  the  grasping  forces  among  the  fingers  making  point 
contact  with  an  object.  This  scheme  reduces  the  nonlinear 
optimization  problem  to  a generalized  eigenvalue  problem. 
They  assumed  that  the  contact  points  between  fingers  and 
object  are  fixed  and  the  contacts  are  hard  point  contacts  with 
friction (it  cannot  transmit  any  torque) . For  the  2-D  case, 
they  concluded  that  the  optimal  grasping  forces,  in  the  sense 
of  minimizing  the  dependence  on  friction,  are  those  for  which 
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the  angles  and  the  resultant  force  have  with  the  corresponding 
normals  are  all  equal (in  absolute  value). 

Nakamura,  Nagai  and  Yoshikawa  [1989]  developed  a method 
of  evaluating  contact  stability  which  is  the  ability  for  the 
end-effector  to  maintain  contact  with  an  object  without 
slipping  when  the  object  is  subjected  to  disturbing  external 
dynamic  forces . 

Yoshikawa  and  Nagai  [1991]  proposed  a new  definition  of 
grasping  and  manipulating  forces  for  multi-fingered  robot 
hands.  The  grasping  force  is  defined  as  an  internal  force 
that  satisfies  the  static  friction  constraint.  Then  the 
manipulating  force  is  defined  as  a fingertip  force  satisfying 
the  following  three  conditions: 

1.  It  produces  the  specified  resultant  force. 

2 . It  is  not  in  the  inverse  direction  of  the  grasping 
force . 

3.  It  is  orthogonal  to  the  grasping  force  component. 

They  also  introduced  the  concept  of  a grasping  focus  to 
verify  grasp  mode  and  to  calculate  grasping  forces.  They  used 
the  following  assumptions: 

1.  Each  fingertip  makes  a frictional  contact  with  the 
rigid  obj  ect . 

2.  The  contact  points  are  not  located  on  a straight  line. 

3.  The  mechanism  of  each  finger  is  such  that  each 
fingertip  can  exert  a force  on  the  object  in  any  direction. 
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1.2.4  Control  of  the  Grip  and  Manipulation 

An  important  area  of  study  in  robotics  is  the  control  of 
the  grip  by  the  end-effector.  The  following  is  a brief 
summary  of  some  of  the  published  papers  in  this  area  which  are 
relevant  to  the  proposed  study. 

Raibert  and  Craig  [1981]  proposed  a hybrid  posit ion/ force 
controller  that  uses  a wrist  mounted  force  sensor  to  control 
the  manipulator  trajectories  in  a task  oriented  Cartesian 
coordinate  system.  The  method  is  a straightforward  approach 
to  the  problem  of  controlling  forces  and  positions 
simultaneously.  Although  this  technique  is  straightforward 
and  feasible,  it  requires  extensive  computations  and  would 
therefore  be  difficult  to  implement  in  real  time  control. 

Arimoto,  Miyazaki  and  Kawamura  [1987]  proposed  a 
cooperative  motion  control  scheme  for  a set  of  multiple  robot 
arm  or  fingers.  This  scheme  is  easily  implemented  in  the  case 
of  a positioning  task  and  it  can  be  extended  to  the  case  of 
path  tracking  control,  force  control,  and  hybrid 
posit ion/ force  control . 

Cole,y  Hauser  and  Sastry  [1988]  considered  the 
manipulation  of  objects  of  arbitrary  shapes  by  multi-f ingered 
hands.  They  treated  the  contact  between  the  object  and  the 
fingers  as  rolling  contacts,  i.e.,  the  fingertip  rolls  without 
slipping  on  the  surface  of  the  object.  They  also  presented  a 
control  law  to  dynamically  control  the  sliding  motion  of  the 
fingertips  along  the  object  surface. 
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Grupen  and  Weiss  [1991]  presented  a multiple  resolution 
object  representation  in  the  force  domain  which  facilitates 
grasp  synthesis.  They  also  proposed  a useful  method  for 
describing  grasp  objectives  and  a control  strategy  for 
designing  the  geometry  of  contact  mediated  by  force  domain 
goals.  They  showed  that  at  a chosen  configuration (null 
space) , the  grasp  forces  can  increase  arbitrarily  without 
producing  net  forces  on  the  object.  The  required  contact 
forces  can  be  made  to  satisfy  the  friction  cone  at  each 
contact  by  squeezing  the  object. 

Payandeh  and  Goldenberg  [1991]  presented  a model  of  the 
manipulator  in  contact  with  a rigid  environment  and  proposed 
a robust  control  architecture  based  on  a general  theory  of 
servomechanism  for  controlling  the  contact  force.  They 
concluded  that  the  controller  can  not  be  made  fast  by 
increasing  the  gain  of  the  servo-compensator  without 
jeopardizing  the  closed-loop  stability  of  the  system. 
However,  the  presence  of  the  compliant  material  between  the 
end-point  of  the  manipulator  and  the  rigid  environment 
introduces  a finite  stiffness  which  allows  the  system  to 
achieve  a fast  and  stable  robust  response. 

1.2.5  Knowledge  Based  Expert  Systems 

There  has  been  considerable  interest  in  recent  years  to 
incorporate  developments  in  artificial  intelligence  and 
knowledge  based  systems  in  the  control  of  the  grip.  The 
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following  is  a brief  summary  of  some  of  the  relevant  studies. 

Randy  C.  Brost  [1988]  presented  an  algorithm  for 
automatic  planning  of  robot  grasping  motions  that  are 
insensitive  to  bounded  uncertainties  in  the  object's  location. 
The  algorithm  plans  parallel-jaw  grasping  motions  for 
arbitrary  two-dimensional  polygonal  objects,  which  need  not  be 
of  uniform  density. 

Stansfield  [1991]  proposed  a knowledge-based  approach  to 
describe  a general  purpose  robotic  grasping  system  for  use  in 
unstructured  environments.  Using  computer  vision  and  a 
compact  set  of  heuristic,  the  system  automatically  generates 
the  robot  arm  and  hand  motions  required  for  grasping  an 
unmodeled  object.  He  implemented  a two-stage  model  of 
grasping.  Stage  one  is  an  orientation  of  the  hand  and  wrist 
and  a ballistic  reach  toward  the  object.  Stage  two  is  hand 
preshaping  and  adjustment.  Visual  features  are  first 
extracted  from  the  unmodeled  object.  These  features  and  their 
relations  are  used  by  an  expert  system  to  generate  a set  of 
valid  reach/grasps  for  the  object. 

A study  which  demonstrates  the  feasibility  of  utilizing 
rule  based  computer  systems  in  regaining  stable  control  after 
it  loses  stability  due  to  fast  occurring  unexpected  external 
effects  is  reported  by  El-Deen  and  Seireg  [1987] . They  showed 
that  the  use  of  a preprogrammed  rule-based  system  can  be  used 
to  improve  a car's  ability  to  perform  safely,  even  when 
traveling  at  high  speeds  on  low-friction  surfaces  by 
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corrective  steering.  They  developed  a computer  control 
algorithm  to  implement  optimized  corrections  based  on  real 
time  sensing  of  the  relevant  variables.  A simulation  of  the 
proposed  real  time  computer'  control  system  showed  that  it 
could  considerably  enhance  the  vehicle  stability  and  ability 
to  maneuver  safely  at  relatively  high  speed  on  icy  roads. 

1.2.6  Sensing 

Manipulator  task  categories  and  motion  phases  require 
various  hand-based  information  systems  to  meet  the  control 
performance  requirements.  One  of  the  early  investigations  in 
this  area  was  undertaken  by  Bejczy  [1977]  . He  presented  the 
effect  of  proximity,  tactile  and  force/torque  sensors  on  the 
performance  of  remote  manipulator  control.  He  also  presented 
an  over  view  on  various  expei'imental  hand-based  information 
systems  which  provide  the  manipulator  controller  with  some 
non-visual  "awareness"  of  the  task  environment.  He  concluded 
that  the  use  of  proximity  sensor  information  can  considerably 
improve  the  performance  of  remote  manipulator  control . He 
suggested  several  means  for  improving  the  performance  of 
manipulator  control.  They  were  as  follows: 

1 . Integrated  display  of  visual  and  proximity  sensor 
information . 

2 . Computer-aided  use  of  proximity  sensor  information  for 


control . 
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3 . Adding  tactile  information  to  the  proximity 


information . 


4 . Introducing  more  dexterous  terminal  devices . 


Dario  and  Buttazzo  [1987] 


presented  the  design, 


implementation,  and  testing  of  an  artificial  sensing  system 
incorporating  an  articulated  robot  finger.  Primary  aim  in 
their  work  was  to  set  up  the  hardware  and  software  tools 
necessary  for  investigating  basic  issues  in  artificial  tactile 


perception . 

Yim  and  Seireg  [1988]  demonstrated  that  a computer- 
controlled  optical  system  provides  a relatively  simple  and 
computationally  fast  technique  for  cooperative  motion 
identification  and  tracking  of  objects.  They  developed  an 
effective  algorithm  for  extracting  the  position  and 
orientation  information  of  the  scan  line  on  a target,  with 
specially  coded  grid  lines,  placed  on  the  surface.  It  can  be 
used  for  performing  and  assembly  or  disassembly  operations  of 
moving  objects  in  manufacturing  and  space  station 
applications . 


1,3  Objective  of  the  Study 

The  objective  of  this  study  is  to  develop  a preprogrammed 
knowledge-based  system  for  adjusting  the  preplanned  grip  in 
order  to  avoid  loss  of  grasp  or  slip  due  to  unexpected 
external  or  internal  conditions  in  the  end  effector's 
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environment.  The  study  includes  the  following  : 

1.  Developing  easily  implementable  control  algorithms  for 
the  optimum  grip  based  on  the  number  of  fingers  and  the 
geometry  of  the  object  to  sustain  the  dynamic  loads  which  are 
to  be  expected  during  the  handling  task. 

2 . Developing  a strategy  for  sensing  any  unexpected 
changes  by  a comprehensive  sensor  system  which  can  monitor  the 
direction  and  magnitude  of  frictional  force  and  normal  force 
at  the  fingertip  or  by  a sensor  system  with  normal  force 
transducers  only  at  the  fingertips. 

3 . Using  the  sensing  information  for  the  identification 

t' 

of  any  unexpected  disturbances  as  they  occur. 

4 . Developing  preprogrammed  control  algorithms  for  the 
best  sequence  of  actions  necessary  to  avoid  impending  slip. 
These  may  include  changes  in  the  normal  forces  applied  by  the 
fingers  as  well  as  rotations  of  the  object  according  to  the 
preprogrammed  algorithm. 

5 . Structuring  a coordinated  control  strategy  to 
incorporate  the  knowledge  base  and  sensor  information  for  real 
time  control.  The  program  would  predict,  initiate  control 
actions,  verify  the  predictions  and  readjust  the  actions  at 
every  time  increment  after  the  sensors  indicate  that  the 
allowable  tolerance  limit  is  violated. 

6.  A computer  simulation  to  investigate  the  feasibility 
of  the  proposed  control  concept  and  any  limitations  on  its 
implementation . 
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7.  The  simulation  will  be  based  on  a three  finger  hand 
configuration  for  handling  a cubical  object  as  an  illustration 
for  proving  the  concept.  Figure  1.1  is  a flow  diagram 
illustrating  the  outline  of  the  control  strategy. 


Figure  1.1  : Flow  Diagram  of  the  Control  Strategy 


CHAPTER  2 

ANALYSIS  OF  SOFT  CONTACT 


2.1  Ideal  Independent  Springs 

2.1.1  Linear  Spring  Model 

A linear  spring  i can  be  characterized  by  its  stiffness 
constant  K± , and  its  direction  which  is  assumed  to  be  oriented 
along  the  normal  to  the  surface  at  the  contact  point  as  shown 
in  Figure  2.1.  A linear  spring  exerts  a pure  forced.  ,e.,  no 
moment)  on  the  object  if  and  only  if  it  is  applied  at  a point 
and  it's  line  of  action  is  along  the  normal  direction. 
Rotations  about  the  contact  point  have  no  effect  on  this  type 
of  spring. 


2.1.2  Angular  Spring  Model 

A soft  finger  contact  can  resist  rotations  of  the  object 
about  the  axis  normal  to  the  surface  at  the  point  of  contact 

(Figure  2.1) . The  considered  angular  spring  K±  exerts  a pure 
moment  if  and  only  if  the  object  is  rotated  about  the  normal 

axis  without  translation. 
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Figure  2.1  Ideal  Springs 


2.2  Stiffness  of  the  Virtual  Spring 

The  interface  between  the  fingertip  and  the  object  can  be 
modelled  as  a system  of  linear  and  angular  springs  attached  at 

the  contact  point  as  in  Figure  2.2.  The  stiffness  of  the  ith 
fingertip  in  the  contact  can  be  represented  by  a matrix  and 
relative  to  a global  reference  frame,  the  stiffness  is 
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k4  0 0 

0 k5  0 

0 0 Kc 


Kx  0 0 


k2  0 


0 

0 0k 


0 


-1 1 


= Ti  Kd  Ti 


-l 


(2.1) 


(2.2) 


where  T±  is  a general  screw  transformation  matrix  of  ith 
contact  given  by  following  equation1. 


lx  mx  nx 
ly  my  ny 
lz  ™Z  nz 
lzy-lyz  mzy-myz 

1xZ~1zX  mxz~mzx 

lyX-lxy  my-mxy 


0 


nzy~nyz 

1 X m X n X 

nxz~nr x 

ly  my  ny 

n.yX-nxy 

}z  mz  nz. 

(2.3) 


The  stiffness  of  the  entire  object  is  the  sum  of  the 
stiffness  matrices 


n 


K = 


- E 

i=l 


(2.4) 


If  the  global  displacement ( twist ) is  known,  the  external 
force (wrench)  is 


1 . Appendix  A 
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W3  = K • t 

where  fc  is  the  twist (displacement  vector). 

t - (bx  by  bz  80x  50y  50z)c 


(2.5) 


(2.6) 


If  the  stiffness  matrix  K is  invertible,  the 
displacement  of  the  object  is 

t = K~1We.  (2.7) 

and  with  respect  to  the  contact  frame,  the  displacement  is 

ti  = TiK'1WB.  (2.8) 

Where  We  is  the  given  external  force (wrench)  including  the 
inertia  force. 

We  = (bfx  bfy  bfz  bmx  bmy  bm^t  (2.9) 


2.3  Relationship  Between  Moment  and  Force 

A common  assumption  in  grasping  analysis  is  that  the 
friction  limits  due  to  torsion  and  shear  are  independent,  so 
that  the  onset  of  slipping  in  rotation  doesn't  depend  on 
applied  tangential  load ( frictional  force) , and  linear  slipping 
doesn't  depend  on  applied  moment.  However,  Howe,  Kao  and 
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Cutkosky  [1988]  measured  on  set  of  slipping  as  a function  of 
applied  moment  and  tangential  force  at  several  different 
normal  forces.  They  suggested  that  if  the  prevention  of  the 
slip  is  of  paramount  concern  in  a manipulation  task,  then  a 
straight  line  drawn  between  the  maximum  moment  and  maximum 
shear  is  a simple,  somewhat  conservative  estimate  of  the 
slipping  limit.  A linear  relationship  between  normal  force 
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and  moment  was  used  by  Badreldin  and  Seireg  [1988,1990]  in 
order  to  minimize  the  maximum  finger  normal  force  by  a linear 
programming  method. 

Both  of  the  above  papers  proposed  a simple  constraint 
equation . 


* ft  + A 


(2.10) 


where  ft  is  the  tangential  frictional  force  on  the  contact, 


ft  = \/(fx2  + 


(2.11) 


is  the  coef  f icient  ■ of  friction,  Fn  is  the  normal  force,  Mn 

is  the  moment,  and  A is  a proportionality  constant  between 
the  torsion  and  shear  limits.  If  the  relationship  between  the 

normal  force  and  contact  area  is  known,  the  constant  A can  be 

derived  as  follows: 

R F ' 

M = JdM  = J 2 it r’dr-  n*— qr '-r  = —”K-R‘\i’Fn/  (2.12) 

0 

A = — (2.13) 

2-n-R 

where  S is  the  area  of  the  contact  at  the  fingertip,  and R 


is  the  radius  of  the  area. 


CHAPTER  3 

OPTIMIZATION  OF  THE  GRASP 
3.1  Introduction 

The  optimization  problem  is  to  find  the  optimal  gripping 
conf iguration ( i . e . , the  location  of  fingers  on  the  surface  of 
the  object)  which  generates  the  minimum  max  normal  force 
between  fingers  and  object  while  moving  it  along  a 
predetermined  path.  For  a particular  grasp  configuration,  the 
maximum  force  of  the  forces  necessary  to  hold  the  object  and 
move  it  along  a predetermined  path  without  slip  can  be 
obtained  by  linear  programming.  The  problem  is  solved  in 
three  steps . 

The  first  step  is  to  find  the  ratios  between  the  finger 
forces,  with  the  smallest  possible  maximum  finger  normal 
force,  which  does  not  produce  any  resultant  wrench  or  twist  of 
the  grasped  object.  This  ratio  should  satisfy  the  frictional 
constraints . 

The  second  step  is  to  determine  the  smallest  incremental 
finger  normal  forces  which  are  required  to  resist  all  the 
external  forces  including  weight  and  inertia  forces. 

In  the  third  step,  the  solutions  from  the  first  and 
second  steps  are  combined  to  determine  the  minimum  multiplier 


22 


23 


for  the  finger  gripping  force  ratio  which  is  necessary  to 
grasp  the  object  without  slipping  at  all  contact  areas. 


In  this  step,  the  object  is  assumed  to  be  weightless.  Its 


the  relationship  between  the  finger  forces  for  any  specific 
gripping  configurations.  These  forces  include  the  normal 
forces  applied  by  the  fingers  as  well  as  all  the  possible 
frictional  forces  and  moments  at  the  contact  areas  between  the 
fingers  and  the  object. 

In  the  general  case,  the  equilibrium  equations  are 
statically  indeterminate  and  linear  programming  is  used  to 
obtain  a solution.  The  linear  objective  function  is  to 
minimize  the  maximum  frictional  components  of  finger  forces. 
It  insures  the  grasp  stability  by  decreasing  its  dependency  of 
the  frictional  components.  The  objective  function  and 
constraints  equations  are  as  follows . 


3.2  The  Static  Gripping  Ratio 


equilibrium  is  defined  by  six  linear  equations  which  define 


Min  (U)  = + A • i^ax 


(3.1) 


subject  to 


n 


(3.2) 


(3.3) 
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fYi\  * fmax 


m 


z i 


^max 


(3.4) 

(3.5) 


-E  s -1 

V — 1 


Where  f„,  , f„,  and  f,  are  the  forces  which  do  not  disturb 

Z JL  AJ.  y -I 

object's  equilibrium,  is  the  normal  force  fxi  and  fyi  are 

perpendicular  frictional  forces  and  m2  is  frictional  moment 
at  ith  contact  point.  ^ is  the  local  wrench  due  to  the 

global  twist,  and  A - is  the  proportionality  constant  between 
the  normal  force  and  moment . 

Wi  = bfy  bfz  5rny  5mz)C±  (3-7) 


The  solution  of  the  gripping  forces  and  moments  is  then 
checked  by  frictional  constraint  equation  to  insure  that  no 
slip  occurs  during  the  initial  grasp. 


yiV2  + (fxi)2)  * 


A ■ Mr 
Z 1 


(3.8) 


If  a solution  does  not  exist,  or  if  the  frictional 
constraint  equations  are  violated,  the  grasp  is  not  attainable 
at  this  specific  grasping  configuration  and  the  finger  contact 
locations  should  be  modified. 
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When  a solution  is  obtained  and  is  found  to  satisfy  the 
frictional  constraints  the  1st  grasping  ratio  is  found  by 
normalizing  the  obtained  solution  as: 

(f W1  = ■ (f«j>  (3.9) 

zmax 

where 

fzmax  = max(fzi  , i = 1,  n)  (3.10) 

If  any  of  the  finger  normal  forces  is  zero  in  the  1st 
grasping  ratio,  another  grasping  ratio  is  to  be  evaluated  in 
which  all  fingers  are  constrained  to  be  active.  The  last 
constraint  equation  is  to  be  substituted  by: 

-fT.  z - 1 i = 1,  n.  (3.11) 

z i 

If  a solution  is  obtained,  it  is  then  normalized  as: 

(?«,)„  - ■ <Fi>  i-l.n  (3.12) 

^ zmax 

If  a solution  does  not  exist,  then  the  only  possible  gripping 
ratio  is  ( F„- ) 1 i = 1,  n . A scala  multiplier  is  then 

/J  Am 

evaluated  for  each  finger  such  that  when  multiplied  by  the 
finger  normal  force  it  would  provide  the  required  frictional 


forces  and  moment . 
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• W + V + P • mzi | 


(3.13) 


The  max  multiplier  is  found  as: 


Psmax  = max(psi,i  = 1 ,n) 


(3.14) 


The  second  gripping  ratio  is  evaluated  using  the  following 
equation : 


the  1st  gripping  ratio  and  the  2nd  normalized  solution. 

The  dynamic  incremental  finger  forces  at  the  contacts  are 
evaluated  by  considering  all  the  locations  along  the 
prescribed  trajectory.  The  resultant  force  due  to  inertia, 


considered  instants  of  time  is  transformed  in  the  object 
frame . 

3.3  Evaluation  of  the  Frictional  Forces  and  Moments 

The  frictional  forces  and  moments  terms  of  a griping 
system  can  be  decoupled  by  assuming  a virtual  spring  system 
which  can  resist  forces  only  along  the  normal  axis  at  the 
contact  points.  In  this  step,  the  required  minimum  frictional 
forces  and  moments  are  obtained  without  considering  the 


max 


weight,  and  external  force  and  moment  vector,  at  the 
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frictional  constraints  by  linear  programming.  The  frictional 
constraints  can  be  adjusted  in  next  step.  The  objective 
function  which  was  used  in  the  previous  step  is  also  used  this 
step.  This  objective  function  enhances  the  grasp  stability  by 
minimizing  the  max  frictional  components  necessary  to  support 
the  grasped  object.  Constraints  in  this  step  are  as  follows. 


E 


+ T..  -W. 


i) 


(3.16) 


fT 

* fr 

xma x 

(3.17) 

fr  * o 

rri 

(3.18) 

f r 1 

ryi| 

^ fr 

x max 

(3.19) 

mr 

rzi 

^ mr 

xnwix 

(3.20) 

Where  Ti,Ki  are  the  general  screw  transformation  matrix  and 
the  stiffness  matrix  of  ith  contact  respectively,  and  ti  , Wz 

are  the  local  twist  and  wrench  due  to  the  external  wrench  We 
respectively.  The  force  components  fT,fr,  and  f are  the 

L x L y L z 

forces  which  do  not  disturb  object's  equilibrium  where  fr  is 

L z 

normal  force  f and  f are  perpendicular  frictional  forces 

f x ry 

and  /i7  is  frictional  moment  at  ith  contact  point. 

z 


28 


3.4  Determination  of  the  Dynamic  Multipliers 


At  this  stage  of  the  procedure,  two  minimum  multipliers 
for  the  gripping  ratios (if  both  exist)  can  be  evaluated  to 

t 

insure  that  no  slip  or  zero  pressure  occur  between  the  fingers 
and  the  object.  These  are  obtained  by  solving  the  following 
frictional  constraint  equations. 


The  maximum  of  each  of  the  two  sets  of  multipliers  are  found 
as  : 


The  actual  grasping  forces  according  to  each  gripping  ratio 
are  therefore  calculated  from: 


The  total  generated  forces  along  the  predetermined  path  are 
evaluated  using  the  following  equation. 


Pmax*  = max(p/,i  = 1,21  k=l , 2) 


(3.22) 


Fgik  - 1=1^  *-1,2 


(3.23) 


(F  k)  = (Fg.)k  + (Fd.)  * 


i=l,n  k= 1,2 


(3.24) 
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The  max  finger  normal  force  that  is  generated  along  the 
predetermined  path  when  any  of  the  two  grasping  ratios  are 
used  to  move  the  object  is  determined  from: 

N k - max (f  k , f k)  (3.25) 

where  f„  K and  f K are  the  normal  components  of  F„  K and  F„  K . 

St  ci  Si  ci 

The  lowest  of  the  two  maximum  normal  forces  generated  along 
the  entire  trajectory  is  determined  from: 

^nax  = min (iW*  ,k=l.2)  (3.26) 

•^nax  determines  which  set  of  forces  are  used  at  the  considered 

gripping  configuration.  The  grasp  ratio  to  be  used  is  the  one 
that  generates  along  the  trajectory. 

The  Figure  3.1.1  and  3.1.2  are  illustrations  of  how  the 
combined  results  of  stepl  and  step2  are  used,  when  the 


frictional  coefficient  is  assumed  to  be  0.1. 
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Static  Ratio 


First  Ratio  Second  Ratio 


Resisting  Force 


i-0  o.i  o.i  i.o 

— > <- 

vl'  sj/ 



0.2 

Combined  Ratio 


Figure  3.1.1  Static  Ratio  and  Resisting  Forces 
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Dynamic  Ratio  (2.5) 


2.5 


Without  Disturbance 


With  Disturbance 


Figure  3.1.2  Dynamic  Ratio 


3.5  Flow  Charts  of  the  Optimization 

Several  computer  programs  in  c language  were  developed  to 
find  the  optimal  configuration  of  three  fingered  gripper  when 
grasping  a cube  or  a sphere  for  any  given  movement . A test 
program  was  also  developed  to  confirm  the  results  of  the 
optimization  programs.  The  optimization  algorithm  follows  the 
same  approach  as  the  one  developed  by  Badreldin  and  Seireg 
[1988,1990] . 

Flow  charts  for  the  total  analysis  and  optimization 
procedure  are  given  in  Figure  3.2  to  3.6. 


Figure  3.2  Basic  Function  Flow 
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Figure  3.3  Static  Ratio 
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Return 


Figure  3.4  Dynamic  Ratio 
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Return 


Figure  3 . 5 


Random  Search 
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Figure  3 . 6 Pattern  Search 
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3.6  Sample  Results 

Sample  results  from  the  optimization  algorithm  based  on 
the  previous  analysis  for  a cube  and  a sphere  are  given  in  the 
following . 

The  inputs  to  the  program  are  the  geometry  of  the  object, 
and  the  external  forces.  The  optimal  contact  points  as  well 

as  the  magnitude  and  the  orientations  of  the  finger  forces  are 

0 

then  calculated.  Illustrative  cases  are  given  below. 

3.5.1  Cube 

Statement  of  the  Problem 

Given  : A cube  is  grasped  by  three  fingers.  Two  of  them 
can  move  on  y-z  planes  and  the  third  on  an  x-z  plane  as  shown 
in  Figure  3.7.  The  coefficient  of  friction  between  the  object 
and  fingers  is  assumed  to  0.4.  The  maximum  external  forces 
and  moments  expected  during  the  task  are  given  as  : 

We  = (0,  -50,  -10,  -0 . 5, 0, 0)fc  (3.27) 

Find  : The  best  three  contact  points  on  the  planes. 

To  Minimize  : The  maximum  finger  normal  force  required  to 
move  the  object  without  slip. 

Optimum  grasp  for  the  weightless  object  taking  into 
consideration  the  entire  dynamic  movement.  The  starting 
condition  and  the  evaluated  optimal  contact  points  and  finger 
force  ratios  are  tabulated  as  follows. 
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Figure  3.7  Cube  Gripped  by  3 Fingers 
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Starting  and  Optimal  contact  positions 


Table  3.1  Starting  Position 


Pi  = c 

-5.0,  0.1,  0.0  ]f 

[cm] 

p2  = [ 

0.0,  -5.0,  0.1  ]r 

[cm] 

P3  = [ 

5.0,  0.2,  0.0  ]f 

[cm] 

Table 

3 . 

2 

Optimal 

Contact  Position 

Pi  = 

[ 

_ c 

3.0,  0.1, 

0.0  ] r [cm] 

/* 

P2  = 

[ 

0 . 

. 0 , -5.0, 

-0 . 8975] 17  [cm] 

11 

[ 

5. 

.0,  0.1, 

0.0  ]'  [cm] 

Optimum  gripping  forces  to  grasp  a weightless  object 
with  coefficient  of  friction  0.4  is  as  follows. 

Table  3.3  Optimum  Forces  ( Friction  Coefficient  = 0.4) 


F>  = [ 

-0.395599, 

-0 .059175, 1 .00 

v 

[N] 

F2  = [ 

0.118350, 

0.0,  0.791197 

r 

■[N] 

f3  = [ 

0.395599, 

-0 . 059175,1 .00 

r 

[N] 
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Finger  Forces  corresponding  to  the  Maximum  External 
Forces . The  optimal  equilibrium  forces  necessary  for 
resisting  the  maximum  external  forces  without  considering  the 
frictional  constraints  are  found  to  be  : 

Table  3.4  Equilibrium  Forces 


Fj  = [ 11.111111, 

6.255787 , 

27.777778 

v 

[N] 

F2  = [ -2.511574, 

0.000000, 

27.777778 

v 

[N] 

F3  = [-11.111111, 

6.255787, 

27.777778 

]f 

[N] 

It  can  be  seen  from  the  table  that  the  frictional 
constraint  is  violated  at  fingers  1 & 3 . 

Optimum  Forces  with  Frictional  Constraints  : Without 

violating  the  frictional  constraints,  the  equilibrium 
finger  forces  required  for  resisting  the  external  forces 
throughout  the  given  task  are  obtained  by  using  the  optimum 
ratio  as  follows. 

Table  3.5  Optimum  Ratio  And  Forces 


Optimum  Ratio  = 74.267064 


Fi  = C 

-29.379953, 

-4.394751, 

74.267064 

v 

[N] 

F2  = [ 

8.789503, 

0.000000, 

58.759906 

r 

[N] 

f3  = [ 

29.379953, 

-4.394751, 

74.267064 

r 

[N] 

3.5.2  Sphere 


Statement  of  the  Problem 

Given  : A sphere (R  = 0.33  m)  is  grasped  by  three  fingers 
on  the  surface  as  shown  in  Figure  3.8.  Since  the  sphere  can 
not  generate  moments  in  any  motion,  the  external  forces  and 
moments  expected  during  a task  are  given  as  : 

We  = (-50,0,0,  -0.5,0, 0)fc  (3.28) 

The  coefficient  of  friction  is  assumed  to  be  0.1  . 

Find  : The  best  three  contact  points  on  the  surface. 

To  Minimize  : The  maximum  finger  normal  force  to  move  the 
object  without  slip. 

Optimum  grasp  for  the  weightless  object  taking  into 
consideration  the  entire  dynamic  movement.  The  starting 
condition  and  the  evaluated  optimal  contact  points  and  finger 
forces  are  tabulated  as  follows. 

Starting  and  Optimal  Contact  Positions 

Table  3.6  Starting  Position  ( R = 0.33  m ) 

Thetal  = 90.0,  Phil  =90.0  [ Deg  ] 

Theta2  = 180.0,  Phi2  =90.0  [ Deg  ] 

Theta3  = 270.0,  Phi3  =90.0  [ Deg  ] 
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Table  3.7  Optimal  Contact  Position  ( r = 0.33  m ) 

Pa  = [ Thetal  = -7.3094,  Phil  = 75.6344  ] [ Deg  ] 

P2  = t Theta2  = 121.0998,  Phi2  = 91.2245  ] [ Deg  ] 

P,  = [ Theta3  = 237.6502,  Phi3  = 90.5219  ] [ Deg  ] 

Optimum  gripping  forces  based  on  weightless  object  is  as 
follows . 

Table  3.8  Optimum  Forces  ( Friction  Coefficient  = 0.1) 

Fa  = [ 0.063877,  0.074035,  0.977836]"  [N] 

F2  = [ -0.074035,  0.066571,  1.000]"  [N] 

F3  = [ 0.012139,  0.074035,  0.979772]"  [N] 
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Finger  Forces  with  the  Maximum  External  Forces 

The  optimum  equilibrium  forces  for  resisting  the  maximum 
external  forces  without  considering  the  frictional  constraints 
are  found  to  be  as  follows. 

Table  3.9  Equilibrium  Forces 

F,  = [ -0.355430,  -0.510909,  0.000000]*  [N] 

F,  = [ 4.527051,  0.521523,  40.342741]*  [N] 

F,  = [ -4.181873,  -1.256450,  40.342741]*  [N] 


It  can  be  seen  that  the  frictional  constraint  is  violated 
at  fingers  1 and  2 in  this  case. 

Optimum  Forces  with  Frictional  Constraints  : Without 

violating  the  frictional  constraints,  the  equilibrium  finger 
forces  for  resisting  the  external  forces  including  inertia 
forces  are  obtained  by  using  the  optimal  multiplier  as 
follows . 

Table  3.10  Optimum  Ratio  And  Forces 


Optimum  Ratio  = 43.542512 


F,  = 

[ 

2.834560, 

3 .285324, 

43.391883 

v 

[N] 

F 2 - 

[ 

-3 .285324, 

2 . 954099, 

44.375434 

]* 

[N] 

F,  = 

[ 

0.538666, 

3 .285324, 

43.477785 

V 

[N] 

CHAPTER  4 

IDENTIFICATION  OF  THE  EXTERNAL  DISTURBANCES 

4.1  Introduction 

An  important  requirement  of  the  computer-based  control 
system  is  the  ability  of  identifying  any  external  disturbance. 
To  identify  the  disturbance  acting  on  the  object,  it  is 
necessary  to  know  the  friction  forces  and  moments  at  all 
fingertips.  If  the  external  disturbance  can  be  identified  at 
any  time,  it  is  always  possible  to  calculate  the  necessary 
reactions  at  the  grip  which  accommodate  the  disturbance.  This 
requires  a sensor  system  which  can  continuously  monitor  the 
magnitude  of  normal  force,  and  magnitude  and  direction  of  the 
friction  force  precisely.  Also,  since  it  takes  some  time  to 
calculate  the  external  disturbance  from  the  sensed 
information,  it  may  be  difficult  to  use  these  sensor  systems 
in  real  time  control.  It  is  envisioned  that  a computer  based 
sensor  system  which  is  fast  enough  and  to  sense  the  normal  and 
friction  force  can  be  developed  for  use  in  real  time  control. 

A real  time  control  strategy  is  highly  dependent  on  the 
sensor  system.  If  the  sensor  system  is  fast  enough  to  monitor 
the  normal  forces  and  friction  forces  precisely,  the  computer 
control  system  can  calculate  the  exact  rotation  of  the  wrist 
and  the  normal  force  which  are  required  to  resist  the 
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disturbance.  Piezo  electric  sensors  at  fingertip  coupled  with 
friction  force  tactile  tangential  sensors  can  be  utilized  in 
this  type  of  sensor  system. 

Since  it  is  difficult  to  directly  sense  the  direction  and 
magnitude  of  the  friction  force,  moment  sensors  at  the  wrist 
can  be  used  to  identify  the  disturbing  force  and  moment  acting 
on  the  object  at  any  instant.  In  this  case,  tactile  sensors 
at  the  fingertip  can  be  used  to  detect  the  direction  of  the 
friction  force  at  each  fingertip.  Normal  force  sensor  and 
tactile  tangential  sensor  at  fingertip  and  moment  sensor  at 
wrist  are  utilized  in  the  different  sensor  systems  considered 
in  the  strategy  discussed  in  this  section. 

The  piezo  electric  meter  considered  as  a normal  force 
sensor  has  good  accuracy  and  fast  response.  If  it  is  possible 
to  use  the  normal  force  information  for  the  control  only,  the 
grip  can  be  easily  adjusted  to  accommodate  the  disturbance  in 
real  time  control  and  it  would  consequently  be  the  best 
approach  to  implement  in  a practical  system. 

Several  computational  algorithms  are  developed  for 
control.  The  first  uses  all  the  information  from  a 
comprehensive  sensor  system  which  can  sense  the  frictional 
force  as  well  as  the  normal  forces.  The  Second  is  based  on 
the  normal  force  information  only  to  resist  the  external 


disturbance . 
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4.2  Sensing  System 
4.2.1  A-Full  Identification 

Initial  State.  When  an  articulated  hand  with  n fingers 
is  performing  a preplanned  operation,  each  finger  exerts  a 
normal  force  with  the  ratio  which  is  necessary  to  sustain  the 

stability  of  the  object  when  subjected  to  expected  forces. 

This  force  ratio  is  obtained  by  using  the  optimization 

procedure  as  explained  in  previous  chapter.  The  fingers  with 

this  force  ratio  do  not  produce  any  net  force  and  moment  on 

the  object  while  no  unexpected  disturbance  is  occurred. 

£ T±  . ' f i = 0 ( i = 1 , 2 ,n)  ( 4 • 1 ) 

i-0 


Ti  is  the  6x6  transformation  matrix  which  transforms 
fingertip  to  the  center  of  the  object  coordinate  system. 


R±  0 
Pi  Ri 


(4.2) 


1 x m x nx 

1 zy~ 1 yz  mzy~myz  nzy-nyz 

Ri  = 

ly  my  ny 

II 

1xZ~1zX  mxZ~mzX  nxZ~nzX 

1z  mz  nz. 

i 

lyX-lxy  irtyX-m xy  riyX-ny/ . 

If  the  position  vectors  of  the  fingertips,  [x  , y and  z in 
equation  (4.3)],  and  the  orientations  of  the  fingertip 
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coordinate  systems,  [ lx,  1 , 1 z,  mx,  mx,  . . , ny,  nz  in  rotation 

matrices]  are  known,  these  transformation  matrices  can  be 
written  as  shown  in  equation  (4.2)  and  (4.3) . 

fl  - 0 , 0 ,m  ] ‘ (4.4) 


The  force  wrench  of  the  ith  finger  fi  is  defined  in  equation 

(4.4)  where  f , f are  the  friction  force  components  in  the 

xi  y± 

x , y direction  of  the  finger  coordinate  system  and  f_  , m 

Z1  zi 

are  the  normal  force  and  moment  components  in  and  about  the  z 
direction  of  the  finger  system  respectively.  Since  the 
fingers  can  not  resist  x and  y components  of  moments,  these 
values  are  set  equal  to  0 . 


When  an  Unexpected  Disturbance  Occurs . If  some 

disturbance  has  taken  place,  the  force  wrenches  of  the  fingers 
will  change  as  a result  of  the  disturbance  as  shown  in 
equation  (4.5)  . 


n 


E 2i  . [ f i + A f , ] 

1=0 


+ f 


ext 


(4.5) 


where 


[AfXi,Afyi,AfZi,  0 , 0 ,A mzy, 


(4.6) 
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f „ = f + f 

ext  exp 


unexp 


^xo 

+ 

fX 

fy0 

+ 

fy 

fz 

"O 

+ 

fz 

+ 

mx 

m 

y o 

+ 

my 

mZ 

m 

+ 

(4.7) 


The  normal  force  change  A fzi  will  be  sensed  from  the  normal 

force  sensor  at  the-£tii  fingertip.  The  change  of  frictional 
forces  at  each  fingertips  in  equation  (4.6)  will  be  also 
sensed  if  there  are  tactile  tangential  sensors.  The 
equilibrium  equation  (4.5)  can  be  rewritten  as 


n 


E r,  . If,  - - f 


ext 


( i = l,  2 n ) 


(4.8) 


i= o 


where 


[bfXi,6fyi,bfZi,  0 , 0 

tfXl+AfXl>  fyi+Afyi mz±+^mz^  C 


(4.9) 


To  be  on  the  conservative  side,  it  is  assumed  that  the 

friction  moment  has  a maximum  value  proportional  to  the 

normal  force  of  the  finger.  If  the  finger  has  constant 
contact  area  at  the  tip,  the  maximum  moment  it  can  resist  will 
increase  proportionally  when  the  normal  force  at  the  contact 


is  increased. 
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Since  all  the  components  of  the  finger  forces  and  moments 
are  known,  the  external  disturbance  f t can  be  readily 

obtained . 

If  the  tactile  sensors  at  each  fingertip  sense  the 
direction  of  the  friction  force  only,  the  moment  sensor  at 
wrist  is  needed  for  the  exact  identification  of  the  external 
interruption.  In  this  case,  equation  (4.9)  can  be  rewritten 
as  equation  (4.10). 

8 f±  = [5fri  cos  (ai)  , 5fri  sin  (ai)  , 8fZi#  0 , 0 , 8/nZj]  t (4 * 10 } 

The  magnitudes  of  friction  forces  fri  and  external  force 
fx  , fy  and  fz  are  the  unknowns  and  sensed  wrist  moments 
mx  , my , mz  , the  directions  of  the  friction  forces  at  each 
fingertips  ai  and  the  normal  forces  fzi  are  the  knowns  in  this 
case.  If  the  number  of  the  fingers  is  3,  there  are  6 

unknowns  in  6 linear  equations  as  before.  The  exact 
disturbance  will  be  readily  evaluated. 

4,2,2  Approximate  Identification 

Since  it  is  generally  difficult  to  incorporate  a tactile 
sensor  which  can  sense  the  exact  magnitude  and  direction  of 
the  friction  force  at  the  fingertip,  an  approximate 
identification  procedure  is  attempted  in  this  section.  If  the 
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sensor  system  consists  of  normal  force  sensors  at  the 
fingertips  and  moment  sensors  at  the  wrist  without  having  any 
tactile  sensor  for  the  friction,  and  the  manipulator  has  three 
fingers,  the  number  of  unknowns  are  9 in  6 linear  equations  in 

this  case.  They  are  three  fxi  , three  f and  fx  , fy  and 

fz  . There  might  be  many  solutions  of  the  unknowns  because 
they  are  underdetermined.  If  any  feasible  solution  is 
obtained,  it  can  be  applied  to  the  control  logic  which  can 
adapt  the  solution  to  be  under  the  control.  Since  the  purpose 
of  this  is  to  get  approximate  external  disturbance,  possible 
maximum  can  be  obtained  after  extensive  computation. 

Another  viable  method  is  introduced  again.  Since  it  is 
under  determined,  several  more  usable  linear  equations  can  be 
acquired  it  will  have  a unique  solution.  It  is  able  to  be 
assumed  that  the  friction  force  isn't  change  a lot  with  very 
small  rotation  of  the  object  while  under  the  same 
interruption.  By  rotating  very  small  angle  about  x,y  and  z 
axis,  3 sets  of  the  linear  equations  can  be  obtained.  To  get 
an  approximate  value  of  the  disturbance,  it  is  possible  to 
choose  one  completed  set  of  the  linear  equations  from  the  3 
sets  when  same  frictional  terms  are  used.  Since  many 
possibilities  should  be  considered  in  this  procedure  it  takes 
time.  In  a real  time  control,  all  the  calculations  at  each 
time  step  must  be  completed  in  a very  short  time  because  all 
the  proper  reactions  also  should  be  accomplished  in  the  time 
step.  If  a fast  enough  computer  and  reactor  such  as  a quick 
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servomotor  are  developed,  these  approximate  methods  can  be 
considered  to  identify  the  external  interruption 
approximately . 


4.2.3 Analysis  of  The  Change  in  Normal  Forces  and  The 

Ability  of  The  Grasp 

i' 

Analysis  of  The  Change  of  The  Normal  Forces.  The 

following  information  can  be  determined  from  the  analysis  of 
the  sensed  normal  forces.  In  the  initial  state,  each  finger 
has  a normal  force  and  friction  force  which  do  not  produce  any 
net  resultant  force  and  moment  on  the  object  if  there  is  no 
unexpected  disturbance.  When  some  external  disturbance 
occurs,  the  normal  force  sensors  detect  the  change  in  the 
normal  forces  due  to  the  disturbance  which  manifests  itself  as 
the  changed  reactions  normal  to  the  contact  areas.  This 
change  quantifies  the  force  ratio  of  the  fingers  which  is 
necessary  to  resist  the  external  disturbance  without  slip  if 
this  is  possible.  The  change  due  to  the  unexpected 
disturbance  can  be  obtained  since  the  expected  disturbance  is 
known . 

Other  useful  information  is  that  the  sum  of  the  normal 
forces  required  to  resist  the  disturbance  and  the  sum  of  the 
moment  due  to  these  normal  forces  for  all  the  fingers  about 
the  center  of  the  obj ect (Origin  of  the  coordinate  system) . 
From  this  information,  a rough  estimation  of  the  external 
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(4.11) 


change  is  possible.  This  will  be  shown  later.  Pi  in 
equation ( 4 . 11 ) is  the  same  3X3  matrix  in  equation  (4.3). 


Evaluation  of  ability  of  the  grasp  to  resist  disturbance. 
The  optimal  grip  evaluated  before  the  disturbance  has  special 
characteristics,  such  as  which  direction  of  the  external 

t' 

disturbance  is  most  or  least  resistible.  This  information 
which  is  known  before  starting  the  correction  indicates  which 
rotating  direction  is  the  most  effective  and  the  one  which  is 
most  detrimental. 


Table  4.1  : Characteristic  of  the  Optimal  Configuration 


AXIS 

FORCE 
( 20.0  N ) 

MOMENT 
(0.5  N-m) 

+ X 

125.188 

53.598 

X 

can't  resist 

1367.440 

+ Y 

33 .484 

104.846 

Y 

24 .198 

104.846 

+ Z 

83.890 

can't  resist 

Z 

45 . 071 

1 79.884 
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Table  4.1  shows  one  example  of  this  for  the  case  of  a 
cube  held  with  three  fingers.  The  table  shows  results 
obtained  after  the  optimal  of  the  grasp  by  applying  arbitrary 
forces  and  moments  to  evaluate  the  ability  of  this  grasp  to 
resist  unexpected  disturbances.  From  the  table,  the  grasp  in 
this  case  cannot  be  maintained  in  the  case  of  some  disturbing 
force  in  negative  X axis  direction  or  a moment  about  positive 
Z axis . 


4.3  The  Utilization  of  The  Sensed  Information 

4.3.1  With  A Comprehensive  Sensor  system 

Extrapolation  of  the  sensed  disturbance  can  be  considered 
for  the  next  step  if  the  sensor  system  can  sense  the  friction 
forces  as  well  as  the  normal  forces  completely  at  fingertips. 
Polynomial  approximation  is  one  of  the  most  effective 
techniques  without  expensive  calculation  time.  If  some 
unexpected  disturbance  is  sensed  for  the  first  time,  a two 
point  linear  approximation  can  be  used  and  a three  point 
quadratic  approximation,  four  point  cubic  approximation  can  be 
used  for  the  next  sequences . 

When  the  sensed  maximum  normal  force  is  greater  than  the 
current  normal  force  in  a finger,  increasing  the  force  ratio 
of  the  grip  immediately  to  the  new  level  corresponding  to  the 
increased  force  in  that  finger  can  prevent  the  object  from 
slipping.  Furthermore,  it  is  possible  to  improve  the  grasp 


54 


for  the  next  time  by  changing  the  orientation  of  the  object 
with  small  angle  for  the  extrapolated  value  . If  the  checked 
normal  force  in  advance  decreases  with  a, small  rotation,  the 
grasp  is  judged  to  be  improving.  If  it  is  increasing  the 
grasp  will  be  worsening.  Following  this  procedure,  it  is 
possible  to  avoid  wrong  rotation  of  the  object  for  the  next 
step.  It  should  be  noted  that  the  resisting  normal  force  will 
increase  sharply  if  the  object  rotates  in  the  wrong  direction. 


4.3.2  With  A System  With  Normal  force  sensors  only 

The  change  of  the  required  normal  forces  and  the  change 
of  the  corresponding  moment  of  these  forces  about  the  center 
of  the  object  can  be  treated  as  a trial  additional  external 
disturbance  for  the  next  time.  Since  the'  sensed  normal  force 
implicates  some  part  of  the  external  disturbance  and  the 
obtained  optimal  configuration  of  the  grasp  was  obtained  based 
on  minimized  maximum  friction  force,  this  can  be  a possible 
way  to  avoid  wrong  rotation  of  the  object  for  the  next  step. 

The  inherent  characteristic  of  the  initial  grasp  is  an 
indicator  of  which  rotating  direction  and  axis  of  rotation 
will  improve  the  grasp  and  which  direction  and  axis  will 
weaken  the  grasp  based  on  the  trial  disturbances  applied 
during  the  evaluation  process  before  the  movement  starts ( as 
illustrated  in  Table  4.1  ) . 
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Estimating  the  Disturbance.  The  solution  vector  can  be 
estimated,  by  using  some  basic  condition.  One  important  step 
is  to  examine  if  any  of  the  fingers  is  slipping  or  shall  slip 
soon  at  this  moment.  One  way  is  to  use  emergency  factor 
defined  in  equation  (4.12)  below.  The  terms  in  the  numerator 
of  the  equation  represent  the  frictional  force  and  moment 
acting  on  the  object  currently.  The  denominator  represents 
the  normal  force  necessary  to  prevent  the  object  from  slipping 
at  this  instant . 


+ 


2 + a 

• H 


(4.12) 


If  Ui  is  greater  than  1 the  finger  is  in  a state  of  slip.  If 

it  is  much  smaller  than  1,  such  as  less  than  0.5,  the  finger 
will  not  slip  at  this  instant.  If  the  finger  is  slipping, 
immediate  reaction,  such  as  increasing  the  normal  force,  will 
be  required  immediately. 


Mar 


f 


z limit 


- f. 


"max 


limit 


(4.13) 


There  can  be  an  upper  normal  force  limit  for  avoiding 

damage  to  the  finger  or  the  object.  Another  factor  Mar 

defined  in  equation  (4.13)  can  be  used  for  determining  how 
much  of  the  finger  normal  force  can  be  added  without  violating 
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the  upper  limit.  Where  fr  in  equation  (4.13)  is  the  maximum 

znax 

normal  force  of  the  fingers.  If  Mar  is  close  to  0,  the 


normal  force  should  not  be  increased. 


CHAPTER  5 
CONTROL  ALGORITHM 

5.1  Introduction 

In  the  previous  chapter,  several  computational  algorithms 
to  identify  the  external  disturbing  force  and  moment  with 
different  degrees  of  certainty  were  introduced.  This  chapter 
addresses  the  question  of  how  to  control  the  manipulator  to 
perform  the  desired  motions  in  the  presence  of  the  unexpected 
disturbances . The  control  algorithms  presented  in  this 
chapter  are  based  on  the  previously  discussed  different 
identification  algorithms.  The  control  for  the  case  of  a 
comprehensive  sensor  system  is  discussed  first,  another  useful 
control  which  relies  only  on  the  input  from  normal  force 
sensors  is  discussed  next. 

An  important  factor  which  is  taken  into  consideration  in 
developing  these  algorithms  is  that  all  computations  and 

reaction  evaluations  should  be  completed  within  the  period 

/ 

needed  for  data  acquisition  by  the  sensor  system.  Despite  the 

fact  that  highly  developed  computer  systems  can  provide  a 

0 

great  advantage  in  computation  time,  there  appears  to  be  no 
suitable  control  algorithms  available  in  the  published 
literature  because  of  the  long  computation  time  which  would  be 
required  for  the  task  under  consideration.  The  generalized 
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optimization  algorithm  for  minimizing  the  maximum  finger 
normal  forces  when  grasping  a moving  object  developed  in 
chapter  3 can  provide  a considerable  advantage  in  computation 
time.  By  using  the  force  ratio  the  required  normal  forces  of 
the  fingers  which  are  necessary  to  maintain  the  stability  of 
the  object  can  be  estimated  immediately  when  an  external 
disturbance  occurs. 

The  possible  corrective  actions  which  are  considered  are 
increasing  the  finger  normal  forces  and  changing  the 
orientation  of  the  object  by  arm  or  wrist  rotations.  Since 
the  starting  optimal  force  ratio  was  obtained  based  on  the 
expected  external  changes  through  out  the  entire  motion,  it 
generally  will  generate  more  finger  forces  than  is  necessary 
to  resist  the  expected  disturbance  at  any  time.  Accordingly, 
when  a relatively  small  unexpected  disturbance  occurs  the 
fingers  may  be  able  to  sustain  the  object  safely.  It  can  also 
be  assumed  that  when  some  disturbance  occurs  for  the  first 
time,  the  object  will  not  start  to  slip  immediately,  and  the 
fingers  can  keep  the  stability  of  the  object  by  adding  the 
required  normal  forces  immediately  if  the  sensor  system  can 
monitor  very  small  changes  as  the  disturbance  progresses.  By 
the  time  it  reaches  the  maximum,  it  is  possible  to  anticipate 
the  progression.  After  adding  the  finger  normal  forces 
instantly  to  resist  the  external  force  and  moment,  the  grasp 
can  be  improved  for  resisting  stronger  disturbances  from  the 
same  direction  by  simply  changing  the  orientation  of  the 
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object  about  appropriate  axes. 

If  the  expected  disturbance  vanishes,  the  grasp  can  then 
be  returned  to  the  original  condition  and  be  prepared  for  the 
next  emergency . 

In  the  case  of  relying  on  normal  force  sensors  only, 
since  the  change  of  the  normal  force  may  be  radical,  the 
extrapolation  can  not  guarantee  proper  values  for  the  next 
step.  The  change  of  the  required  normal  forces  and  the  change 
of  the  corresponding  moment  of  these  forces  about  the  center 
of  the  object  can  be  treated  as  a trial  additional  external 
disturbance  for  the  next  time  as  explained  in  previous 
chapter.  If  the  unexpected  disturbance  is  stronger,  the 
sensed  normal  force  differential  will  constitute  a larger 
segment  of  the  total  signal  and  this  method  becomes  more 
reliable  under  such  condition. 

5.2  Control  with  a Comprehensive  Sensor  System 

A comprehensive  sensor  system  detects  all  the  information 
about  the  change  in  normal  and  the  tangential  friction  forces 
at  the  fingertips  resulting  from  any  external  disturbance. 
Such  a system  would  incorporate  tactile  sensors  which  can 
sense  the  magnitude  and  direction  of  the  friction  forces  and 
normal  force  sensors  at  fingertips.  If  the  tactile  sensors 
are  not  sufficient  for  evaluating  the  direction  and  the 
magnitude  of  the  frictional  force  at  the  fingertips,  moment 
sensors  at  the  wrist  would  be  needed  to  supplement  the 
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shortage  of  the  information.  Since  the  optimal  grasp  is  a 
preplanned  operation,  the  expected  external  force  and  moment 
can  be  predicted  completely  and  the  grip  can  be  prepared  to 
accommodate  all  the  known  external  change  when  there  is  no 

y 

unexpected  external  disturbing  force  or  moment . Since  a 
unexpected  disturbance  can  be  calculated  from  the  detected 
information  as  in  equation  (4.6),  it  also  is  possible  to 
foresee  the  best  orientation  which  improves  the  grasp  by 
virtual  change  of  the  orientation  about  all  the  axes  to 
positive  and  negative  direction  with  the  extrapolated 
estimates  of  the  disturbance.  If  some  unexpected  disturbance 
is  detected,  prompt  adding  of  the  required  normal  forces  of 
the  fingers  is  necessary  first  and  finding  the  best 
orientation  of  the  object  is  the  next  action.  If  the 
unexpected  external  disturbance  is  changing  it  is  necessary  to 
forecast  the  disturbance  which  will  occur  next  by 
extrapolating  the  detected  change. 

The  main  advantage  with  this  complete  sensor  system  is 
that  the  grip  can  resist  all  the  resistible  disturbance  by 
increasing  the  exact  amount  of  the  required  normal  forces  and 
changing  the  orientation  of  the  object  in  a deterministic 
manner  without  the  possibility  of  estimation  errors  which  can 
be  the  case  if  the  manipulator  is  not  equipped  with  a 
comprehensive  sensor  system. 
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5.3  Control  with  Normal  Force  Sensors  Only 

A sensor  system  which  has  normal  force  sensors  only  can 
detect  only  information  about  the  normal  forces  at  the 
fingertips  which  result  from  the  external  disturbance.  Since 
this  is  a preplanned  operation,  the  expected  external  force 
and  moment  can  be  resisted  completely.  The  grasp,  however,  is 
not  expected  to  be  able  to  accommodate  any  temporary  external 
change  without  a comprehensive  sensor  system  if  there  is  some 
unexpected  external  disturbance.  Since  the  unexpected 
external  disturbance  can  not  be  calculated  deterministically 
from  the  detected  normal  force  information,  it  is  not  there 
fore  possible  to  foresee  the  best  orientation  of  the  object  to 
improve  the  grasp  and  prevent  slip. 

The  sum  of  the  normal  forces  required  to  resist  the 
disturbance  and  the  sum  of  the  moments  due  to  these  normal 
forces  for  all  the  fingers  about  the  center  of  the  object 
(Origin  of  the  coordinate  system)  as  obtained  from  equation 
(4.13)  is  considered.  From  this  information,  a rough 
estimation  of  the  external  change  is  possible.  A negative 
change  of  these  values  can  be  treated  as  a known  disturbance 
so  that  it  is  possible  that  the  sum  of  the  these  values  and 
the  expected  disturbance  can  be  used  as  the  completely 
detected  disturbance  as  in  the  case  of  the  control  with  a 
comprehensive  sensor  system  for  the  next  step. 

The  change  of  the  normal  force  due  to  the  unexpected 
disturbance  can  be  instantaneously  detected.  If  some  change 
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is  detected,  it  is  necessary  first  to  immediately  apply  the 
required  normal  forces  of  the  fingers (as  long  as  it  is  below 
the  allowable  limit),  and  then  finding  the  best  orientation  of 
the  object  as  the  next  step. 

#' 

In  this  case  it  is  possible  to  foresee  a better  (not  the 
best)  rotation  of  the  object  to  improve  the  grasp  by 
incremental  rotating  the  wrist  about  all  the  axes  in  the 
positive  and  negative  directions.  Consequently  it  is  possible 
to  avoid  unacceptable  large  increase  of  the  normal  forces . 


5,4  Possible  States 

After  the  preplanned  operation  is  started,  it  is  assumed 
that  an  unexpected  disturbance  can  occur  at  any  time.  It  can 
occur  before  or  after  the  expected  forces  take  place. 
According  to  the  time  when  an  unexpected  disturbance  occurs, 
the  control  of  the  grasp  should  be  prepared  for  the  change. 
When  the  preplanned  operation  starts  it  can  be  assumed  that 
expected  and  unexpected  disturbances  has  not  yet  occurred.  At 
this  state,  the  grasp  prepares  for  the  expected  force  and 
moment  which  will  occur  during  the  operation  by  generating  the 
required  normal  forces  according  to  the  precalculated  force 
ratio  from  the  generalized  optimization  procedure.  The  sensor 
system  starts  monitoring  every  change  of  the  sensed 
information.  If  the  end  effector  has  a comprehensive  sensor 
system  the  information  will  be  used  in  the  extrapolation  for 
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the  next  step.  If  the  fingers  have  normal  force  sensors  only, 
the  change  of  the  normal  forces  will  be  used  for  the  next 
step.  Algorithms  from  this  initial  state  to  other  states 
should  be  activated  at  this  moment.  All  the  events  which  can 
take  place  during  the  operation  and  the  control  algorithms 
according  to  these  events  are  as  follows  (Figure  5.1) . 


Initial  State  : After  the  start  of  the  operation,  no 

disturbance  has  occurred  yet. 


State  A : From  the  Initial  state,  some  unexpected 

disturbance  is  detected  while  no  expected  disturbance 
occurred . 


State  B 


From  the  Initial  state,  some  expected 


disturbance  is  detected  yet  no  unexpected  disturbance 
occurred . 


State  C : From  the  state  A or  B,  some  expected  and 


unexpected  disturbance  are  detected. 
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Figure  5.1 


Possible  States 
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5.5  Control  Algorithms 

The  control  algorithms  for  these  states  are  as  follows. 

5.5.1  Algorithms  for  the  Initial  State 

Grasp  the  object  with  the  initial  ratio  and  start 
monitoring  the  sensed  information. 

If  unexpected  disturbance  is  not  sensed,  keep  the 
original  orientation  even  though  the  expected  disturbance  has 
occurred. 

When  any  unexpected  disturbance  is  detected. 

1.  Change  the  force  ratio  instantly  to  generate  the 
appropriate  normal  forces. 

2 . Anticipate  the  next  disturbance  from  the 
monitored  information,  calculate  the  normal  force  ratio 
and  find  the  rotation  axes  and  directions  which  will 
improve  the  grip  for  the  next  step. 

3.  Rotate  the  object  about  the  axes  for  the  next 

step . 

Add  the  information  about  the  expected  disturbance 
just  before  the  disturbance  occurs  for  the  next  step. 

The  first  algorithm  can  be  used  at  any  time  if  some  exist 
disturbance  has  disappeared.  This  is  especially  important 
when  the  grasp  is  in  the  state  A and  state  C.  Without  this 
algorithm  the  manipulator  can  not  be  prepared  for  the  next 
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expected  disturbance  from  these  states.  Once  the  sensor  system 
starts  monitoring  the  sensed  information,  it  will  last  until 
the  manipulator  finishes  the  operation. 

The  second  algorithm  is  for  continuing  the  state  or  for 
reaching  state  B from  the  initial  state.  Since  the  grasp  has 
already  been  prepared  well  for  the  expected  disturbance,  the 
manipulator  can  sustain  the  stability  of  the  object  without 
changing  the  force  ratio  or  the  orientation  of  the  object. 

The  third  algorithm  is  for  reaching  state  A from  the 
initial  state.  This  can  also  be  used  when  the  disturbance  is 
changing.  It  is  the  most  difficult  situation  when  the  state 
changes  from  the  initial  state  to  the  state  A.  If  the  grasp 
can  not  resist  this  unexpected  disturbance  when  it  is  detected 
for  the  first  time,  the  manipulator  can  not  perform  the 
operation  successfully.  This  can  occur  when  the  disturbance 
is  too  large  or  it  is  from  a direction  the  grasp  cannot 
resist.  The  human  hand  also  cannot  hold  an  object  in  this 
type  of  situation. 

The  fourth  algorithm  is  for  reaching  state  B from  this 
initial  state.  Because  of  the  conditions  for  the  initial 
state,  the  grasp  can  be  prepared  for  a sharp  increase  of  the 
required  normal  forces  due  to  the  expected  disturbance  during 
this  initial  state. 
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5.5.2  Algorithms  for  the  State  A 


If  the  detected  unexpected  disturbance  is  variable. 

1.  Change  the  force  ratio  instantly  to  generate  the 
appropriate  normal  forces . 

2 . Anticipate  the  next  disturbance  from  the 
monitored  information,  and  calculate  the  normal  force  ratio 
and  find  the  rotation  axes  and  directions  which  will  improve 
the  grip  for  the  next  step. 

3.  Rotate  the  object  about  the  axes  for  the  next 


step . 

If  the  detected  disturbance  is  not  changed  from  the 
previous  step,  keep  the  current  normal  force  ratio  and 
orientation  of  the  object. 

Add  the  information  of  the  expected  disturbance  just 
before  the  disturbance  occurs  for  the  next  step. 

If  the  detected  disturbance  ends  return  to  the 
initial  state. 


The  first  algorithm  is  exactly  the  same  that  the  third 
one  in  the  initial  state.  If  the  unexpected  disturbance  is 
changing  rapidly  and  the  grasp  can  not  accommodate  it,  the 
system  may  become  unstable. 

The  second  algorithm  is  always  effective  when  the  sensed 
unexpected  disturbance  does  not  change. 
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The  third  algorithm  is  fox'  reaching  state  C from  state  A 
and  this  algorithm  is  always  effective  if  the  expected 
disturbance  is  beginning  to  occur. 

The  fourth  algorithm  is  for  reaching  the  initial  state 
from  state  A. 

t' 

5.5.3  Algorithms  for  the  State  B 

If  no  unexpected  disturbance  is  sensed,  keep  the 
original  orientation  even  though  expected  disturbance  has 
occurred . 

When  any  unexpected  disturbance  is  detected. 

1.  Change  the  force  ratio  instantly  to  generate  the 
appropriate  normal  forces. 

2 . Anticipate  the  next  disturbance  from  the 
monitored  information,  calculate  the  normal  force  ratio  and 
find  the  rotation  axes  and  directions  which  will  improve  the 
grip  for  the  next  step. 

3.  Rotate  the  object  about  the  axes  for  the  next 

step . 


Algorithms  in  this  state  are  the  same  as  those  for  the 
initial  state  because  the  operation  is  preplanned  and  the 
grasp  has  been  prepared  for  this  state.  And  the  second 
algorithm  is  for  reaching  state  C from  state  B. 
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5.5.4  Algorithms  for  the  State  C 

If  the  unexpected  disturbance  is  changing. 

1.  Change  the  force  ratio  instantly  to  generate  the 
appropriate  normal  forces . 

2 . Anticipate  the  next  disturbance  from  the 
monitored  information,  and  calculate  the  normal  force  ratio 
and  find  the  rotation  axes  and  directions  which  will  improve 
the  grip  for  the  next  step. 

3.  Rotate  the  object  about  the  axes  for  the  next 

step . 

If  the  unexpected  disturbance  ends,  return  to  the 
initial  state  and  perform  the  algorithms : for  that  state. 

Add  the  information  about  the  expected  disturbance 
just  before  the  disturbance  occur  for  the  next  step. 

The  second  algorithm  is  for  the  state  B and  the  third 
algorithm  is  for  reaching  state  B or  the  initial  state  from 
state  C respectively. 

All  the  algorithms  corresponding  to  each  state  can  be 
combined  for  the  entire  control  operation  which  can  maintain 
the  stability  during  the  operation  for  all  the  circumstances 
if  the  disturbances  are  resistible.  Sometimes  the  manipulator 
can  accommodate  some  disturbance  which  are  not  resistible. 
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If  the  system  has  a comprehensive  sensor  system  and  the 
disturbances  are  resistible,  it  is  always  possible  to  maintain 
the  stability  during  the  operation. 

Several  other  algorithms  can  be  considered  such  as  : 

Choose  the  one  best  rotation  for  improving  the  grasp 
to  reduce  the  reaction  time.  , 

If  the  system  has  a comprehensive  sensor  system  and 
the  only  current  disturbance  is  unexpected,  compare  the 
required  normal  force  with  the  current  orientation  and  with 
the  original  orientation  for  the  disturbance,  and  then  choose 
the  better  orientation  based  on  the  comparison. 


5.5.5  Algorithm  for  Finding  the  Best  Orientation 

The  algorithm  for  finding  the  best  orientation  which  is 
required  in  all  the  states  uses  most  of  the  computation  time 
during  the  procedures.  Rotation  matrix  which  represents  the 
orientation  of  the  object  at  the  moment  is  calculated  every 
step.  Since  the  different  order  of  rotations  produces 
different  orientation,  it  is  required  to  choose  the  order  of 
rotations.  Following  is  the  algorithm  for  finding  the  best 
orientation  by  trial  rotations. 

1.  Calculate  the  required  normal  force  ratio  for  the 
anticipated  disturbance  which  will  occur  next  time  with 


current  orientation. 
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2.  Rotate  to  the  positive  and  negative  direction  by  a 
step  degree  about  each  axis  and  check  if  the  required  force 
ratio  is  increasing. 

3.  For  each  axis,  choose  the  value  1.0  if  the  required 
force  ratio  is  decreasing  with  positive  rotation,  or  choose 
-1.0  if  the  required  force  ratio  is  decreasing  with  negative 
rotation,  or  choose  0.0  if  there  is  no  improvement. 

4.  Make  a rotation  matrix  with  a chosen  order. 

5.  Multiply  the  rotation  matrix  before  the  current 
rotation  matrix,  and  check  if  it  is  improving. 

6.  If  it  is  improving,  repeat  previous  step  5 until  it 
isn't  improving. 

The  control  algorithms  introduced  here  are  very  simple 
and  easy  to  implement  in  a real  time  control.  Some  results  of 
several  practical  examples  will  be  shown  and  discussed  in  the 


next  chapter. 


Figure  5.2  Initial  State 


Figure  5.3 


State  A 


Figure  5.4 


State  B 


From  State  A or  B 


Figure  5.5  State  C 


CHAPTER  6 

SIMULATION  RESULTS  AND  DISCUSSION 

6.1  Introduction 

To  examine  the  feasibility  of  these  control  algorithms, 
several  computer  simulation  examples  are  run  for  illustration 
and  the  results  are  given  in  Figure  6.9  through  Figure  6.58. 
The  assumed  situations  include  all  the  possible  states  are 
explained  in  previous  chapter.  In  the  considered  examples, 
several  unexpected  pulse  disturbances  are  activated  while  the 
manipulator  is  performing  the  preplanned  operation.  The 
unexpected  disturbances  are  shown  in  Figure  6.6  to  6.8  and 
they  represent  both  force  and  moment  disturbances . The  data 
acquisition  time  increment  during  the  simulation  is  assumed  to 
be  0.01  sec. 

The  cube  model  discussed  in  chapter  3 is  used  as  the 
object  since  the  cube  is  the  most  difficult  one  among  those 
models.  The  optimal  positions  of  the  fingers  and  the  required 
normal  force  ratio  for  the  three  expected  disturbances  during 
the  preplanned  operation  are  as  listed  in  Table  6.1. 

The  results  for  the  simulations  with  comprehensive  sensor 
system  will  be  explained  first  and  those  with  normal  force 
sensors  only  will  be  discussed  later. 
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Table  6.1  Starting  Condition 
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OBJECT  : CUBE  ( 0 . 10  x 0 . 10  x 0 . 10  ) [m] 

FRICTIONAL  COEFFICIENT  : 0.3 

THREE  EXPECTED  DISTURBANCES  : FORCE; [N] , MOMENT; [N-m] 


Fi  = 

O 

• 

o 

-30.0, 

V. 

O 

• 

o 

o 

• 

o 

0.3, 

o 

• 

o 

f2  - 

[ o.o, 

0.0, 

1 

l-> 

o 

• 

o 

o 

• 

o 

o 

• 

o 

0.0  ] 

F3  = 

[-20.0, 

o 

• 

o 

CM 

** 

o 

• 

o 

LD 

• 

o 

** 

o 

• 

o 

0.0  ] 

OPTIMAL  FINGER  POSITION  FOR  THE  DISTURBANCES  : 
P,  = [ -0.05,  0.0,  0.00  T [ m ] 

P2  = [ 0.0039,  -0.050,  0.014  ]r  [ m ] 

P3  = [ 0.05,  0.0,  0.00  ]c  [ m ] 

REQUIRED  NORMAL  FORCE  RATIO  = 37.582  N 


The  assumed  unexpected  situations  for  the  computer 
simulations  are  as  follows. 

Situation  1 : Three  different  unexpected  disturbances 

occur  with  no  expected  disturbance.  ( Figure  6.1  ) 

Situation  2 : Three  unexpected  disturbances  occur  in 

conjunction  with  three  different  expected  disturbances 
respectively.  ( Figure  6.2  ) 

Situation  3 : Three  unexpected  disturbances  occur  in 

conjunction  with  an  expected  disturbance.  ( Figure  6.3  ) 

Situation  4 - 5 : Same  as  situation  2 and  3 but  with 

higher  unexpected  disturbances.  ( Figure  6.4  to  6.5  ) 
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Figure  6.1  Situation  1 
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Figure  6.2  Situation  2 
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Fy  = -30 
My  = 0.3 


: Un Expected 


: Expected 


Figure  6.3 


Situation  3 
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Figure  6.4  Situation  4 
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Figure  6 . 5 


Situation  5 


MOMENT  [ 
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UNEXPECTED  DISTURBANCE 


FORCE  ALONG  Z AX  I S 


Figure  6.6  The  First  Unexpected  Disturbance 


UNEXPECTED  DISTURBANCE 


MOMENT  ABOUT  Y AX  I S 


Figure  6.7  The  Second  Unexpected  Disturbance 


MOMENT  [ 
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UNEXPECTED  DISTURBANCE 


MOMENT  ABOUT  Z AX  I S 


Figure  6.8  The  Third  Unexpected  Disturbance 
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6.2  Response  With  A Comprehensive  Sensor  system 


Situation  1 ( See  Figure  6.9  to  6.14) 


Since 

there 

is  no 

expected 

disturbance 

during  the 

operation, 

these 

examp 1 e 

show  how 

the  system 

works  if  an 

unexpected  disturbance  occurs  without  any  anticipated  forces 
acting  on  the  object.  The  system  maintains  its  equilibrium 
during  the  force  disturbance.  The  grip  holds  the  object  with 
a very  small  change  of  the  orientation.  The  algorithm  becomes 
very  active  when  the  negative  moment  disturbance  along  y axis 
occurs.  The  required  normal  force  is  considerably  reduced  as 
a result  of  the  moment  disturbances  when  compared  with  the 
required  normal  force  in  the  case  without  changing 
orientations  as  shown  in  Figure  6.9. 

Situation  2 ( See  Figure  6.15  to  6.20  ) 

Since  this  situation  is  assumed  to  examine  the  response 
during  the  most  difficult  circumstances,  very  active  reaction 
is  to  be  expected.  The  required  normal  forces  are 
considerably  reduced  by  changing  the  orientation  especially 
when  the  second  pulse  was  stimulated.  The  change  of  the 
required  normal  forces  is  very  high  because  the  disturbing 
input  is  changing  radically  in  a very  short  period. 
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Situation  3 ( See  Figure  6.21  to  6.26  ) 

This  situation  is  one  of  the  most  common  circumstances 
when  considering  a real  preplanned  operation.  The  grasp  is 
very  stable  during  the  entire  period  of  operation.  Also  the 
system  can  sustain  the  stability  of  the  object  without 
changing  the  orientation  in  this  case. 

Situation  4 ( See  Figure  6.27  to  6.32  ) 

Since  the  unexpected  disturbances  are  stronger  than 
situation  2 and  3,  the  system  can  not  resist  any  unexpected 
disturbance  without  rotating  corrections  in  this  case. 
Because  the  change  of  the  orientation  at  each  step  is  rapid, 
the  control  should  have  very  quick  response  characteristics. 


Situation  5 ( See  Figure  6.33  to  6.38  ) 

Even  though  the  unexpected  disturbance  is  larger  than  the 
disturbance  in  the  situation  3,  the  difficulties  in  the  change 
of  required  normal  forces  and  of  rotating  the  object  are  not 
significantly  changed.  The  difficulties  of  both  cases  are 


almost  same. 
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REQUIRED  NORMAL  FORCESC  Situation  1 } 

With  Comprehensive  Sensor  System 


Figure  6.9  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  1 } 

With  Comprehensive  : About  X axis 


□ Q.5  1 1.5  2 


TIME  [SEC] 


Figure  6.10  Change  of  The  Orientation  : About  X axis 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  1 } 


0 Q . 5 1 1.5  2 


TIME  [SEC] 

Figure  6.11  Change  of  The  Orientation  : About  Y axis 


CHANGE  OF  THE  OR  I ENTAT i ONC  Situation  1 } 


0.5 


1.5 
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Figure  6.12  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  1 } 
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Figure  6.13  Required  Normal  Forces  Without  Rotation 


REQUIRED  NORMAL  FORCE  RAT  I OC  Situation  1 } 
With  Comprehensive  Sensor  System 


Figure  6 . 14 


Required  Normal  Force  Ratio 
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With  Comprehensive  Sensor  System 


Figure  6.15  Required  Normal  Forces 
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Figure  6.16  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  2 } 

With  Comprehensive  : About  Y axis 
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Figure  6.17  Change  of  The  Orientation  : About  Y axis 
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Figure  6.18  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  2 } 

With  Comprehens i ve  : Without  Rotation 


Figure  6.19  Required  Normal  Forces  Without  Rotation 


REQUIRED  NORMAL  FORCE  RAT  I OC  Situation  2 1 
With  Comprehensive  Sensor  System 


Figure  6.20 


Required  Normal  Force  Ratio 
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Figure  6.21  Required  Normal  Forces 
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Figure  6.22  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  3 } 

With  Comprehensive  : About  Y axis 


Figure  6.23  Change  of  The  Orientation  : About  Y axis 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  3 } 

With  Comprehensive  : About  Z axis 


Figure  6.24  Change  of  The  Orientation  : About  Z axis 


REQUIRED  NORMAL  FORCESC  Situation  3 } 

With  Comprehensive  : Without  Rotation 


Figure  6.25  Required  Normal  Forces  Without  Rotation 


REQUIRED  NORMAL  FORCE  RAT  I OC  Situation  3 } 
With  Comprehens i ve  Sensor  System 


Figure  6.26  Required  Normal  Force  Ratio 
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REQUIRED  NORMAL  FORCESC  Situation  4 3 

With  Comprehensive  Sensor  System 


Figure  6.27  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENT AT  I ONC  Situation  4 } 
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Figure  6.28  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  4 3 

With  Comprehensive  : About  Y axis 
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Figure  6.29  Change  of  The  Orientation  : About  Y axis 
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Figure  6.30  Change  of  The  Orientation  : About  Z axis 
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Figure  6.31  Required  Normal  Forces  Without  Rotation 


REQUIRED  NORMAL  FORCE  RAT  I OC  Situation  4 } 
With  Comprehensive  Sensor  System 


Figure  6.32 


Required  Normal  Force  Ratio 
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REQUIRED  NORMAL  FORCESC  Situation  5 } 

With  Comprehensive  Sensor  System 


Figure  6.33  Required  Normal  Forces 
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Figure  6.34  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  5 } 

With  Comprehens i ve  : About  Y axis 


Figure  6.35  Change  of  The  Orientation  : About  Y axis 
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Figure  6.36  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  5 } 

With  Comprehensive  : Without  Rotation 


Figure  6.37  Required  Normal  Forces  Without  Rotation 
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Figure  6.38  Required  Normal  Force  Ratio 
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6.3  Response  With  Normal  Force  Sensors  only 

Situation  1 ( See  Figure  6.39  to  6.42  ) 

Because  the  system  is  not  able  to  evaluate  the  exact 
disturbance,  the  produced  required  normal  forces  are  higher  in 
this  case  than  in  the  cases  of  where  a comprehensive  sensor 
system  is  used.  The  changing  rate  of  the  orientation  is 
slightly  unstable  especially  when  the  unexpected  force  pulse 
is  induced.  However,  when  the  moment  pulse  is  induced,  the 
system  responses  are  very  stable.  The  changes  of  the 

orientation  are  relatively  small  and  the  change  of  the 
required  normal  forces  are  also  relatively  small. 

Situation  2 ( See  Figure  6.43  to  6.46  ) 

Since  the  disturbances  are  relatively  small  in  this  case, 
the  system  can  sustain  the  object  with  a required  normal 
forces  less  than  70  N.  The  changing  rates  of  the  orientation 
also  show  reasonable  ranges  during  the  operation. 

Situation  3 ( See  Figure  6.47  to  6.50  ) 

Since  the  system  has  normal  force  sensors  only,  it  is 
expected  that  the  required  normal  forces  will  be  much  higher 
than  the  system  which  has  a comprehensive  sensor  system. 
However,  the  results  for  this  case  are  as  good  as  the  results 
with  the  comprehensive  system.  From  these  results  it  can  be 
stated  that  the  sensor  system  which  has  normal  force  sensors 
only  is  satisfactory  when  the  unexpected  disturbances  are 
relatively  small. 
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Situation  4 ( See  Figure  6.51  to  6.54  ) 

Since  the  disturbances  are  relatively  larger  than  the 
previous  situations,  the  required  normal  forces  are  higher 
than  before  but  still  are  in  the  feasible  ranges.  The  normal 
forces  which  can  sustain  the  stability  of  the  object  is 
considerably  reduced  by  the  rotating  corrections  as  can  be 
seen  in  Figure  6.51. 

Situation  5 ( See  Figure  6.55  to  6.58  ) 

Despite  the  fact  that  the  disturbances  are  relatively 
large  in  this  case,  the  system  is  very  stable  when  the 
unexpected  disturbances  occur  in  conjunction  with  some 
expected  disturbance.  The  changing  rate  of  the  orientation  is 
small  during  the  operation  and  the  required  normal  forces  are 
less  than  70  N except  for  only  one  instant. 
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REQUIRED  NORMAL  FORCESC  Situation  1 3 

With  Normal  Force  Sensors  Only 


Figure  6.39  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENT AT  I ONC  Situation  1 } 

With  Normal  Force  Sensors  Only  : About  X axis 


Figure  6.40  Change  of  The  Orientation  : About  X axis 


105 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  1 } 

With  Normal  Force  Sensors  Only  : About  Y axis 


0 0.3  1 1.3  2 


TIME  [SEC] 

Figure  6.41  Change  of  The  Orientation  : About  Y axis 
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With  Normal  Force  Sensors  Only  : About  Z axis 
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Figure  6.42  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  2 } 
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Figure  6.43  Required  Normal  Forces 


CHANGE  OF  THE  ORIENTATIONC  Situation  2 } 

With  Normal  Force  Sensors  Only  : About  X axis 


Figure  6.44  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  2 } 

With  Normal  Force  Sensors  Only  : About  Y axis 
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Figure  6.45  Change  of  The  Orientation  : About  Y axis 
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Figure  6.46  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  3 3 


With  Normal  Force  Sensors  Only 


Figure  6.47  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENT AT  I ONC  Situation  3 } 

With  Normal  Force  Sensors  Only  : About  X axis 


Figure  6.48  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  3 } 


With  Normal  Force  Sensors  Only  : About  Y axis 


Figure  6.49  Change  of  The  Orientation  : About  Y axis 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  3 } 

With  Normal  Force  Sensors  Only  : About  Z axis 


Figure  6.50  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  4 } 


With  Normal  Force  Sensors  Only 


Figure  6.51  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  4 } 


With  Normal  Force  Sensors  Only  : About  X axis 


Figure  6.52  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  4 3 

With  Normal  Force  Sensors  Only  : About  Y axis 
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Figure  6.53  Change  of  The  Orientation  : About  Y axis 
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Figure  6.54  Change  of  The  Orientation  : About  Z axis 
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REQUIRED  NORMAL  FORCESC  Situation  5 } 

With  Normal  Force  Sensors  Only 


Figure  6.55  Required  Normal  Forces 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  5 } 

With  Normal  Force  Sensors  Only  : About  X axis 


Figure  6.56  Change  of  The  Orientation  : About  X axis 
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CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  5 } 

With  Normal  Force  Sensors  Only  : About  Y axis 


Figure  6.57  Change  of  The  Orientation  : About  Y axis 


CHANGE  OF  THE  OR  I ENTAT I ONC  Situation  5 } 

With  Normal  Force  Sensors  Only  : About  Z axis 


Figure  6.58  Change  of  The  Orientation  : About  Z axis 


6.4  Conclusions 


The  system  which  has  normal  force  sensors  only  is 
satisfactory  when  the  moment  disturbances  occur  at  a time 
without  any  expected  disturbance. 

The  system  with  a comprehensive  sensor  system  can 
always  sustain  the  stability  of  the  object  if  the  unexpected 
disturbance  which  is  detected  at  first  is  resistably  at  that 
instant . 

If  the  object  can  sustain  large  normal  forces  without 
damage,  a system  with  normal  force  sensors  only  can  be 
utilized  and  higher  normal  forces  will  generally  be  estimated 
to  resist  the  unexpected  disturbances. 

By  monitoring  the  emergency  factor  which  is  defined 
in  chapter  4,  the  manipulator  can  add  the  required  normal 
force  instantly  in  a real  time  control.  If  the  system  has  a 
value  of  the  factor  less  than  1.0  and  does  not  reduce  the 
normal  forces  once  they  are  increased,  rapid  change  of  the 
normal  force  can  be  avoided  as  illustrated  in  the  Figures. 


CHAPTER  7 

SUMMARY, CONCLUSIONS  AND  RECOMMENDATIONS 

7 . 1 Summary 

In  the  present  work,  control  algorithms  for  an  articulate 
manipulator  to  sustain  the  stability  of  the  grasped  object  are 
developed  to  deal  with  situations  where  some  unexpected 
disturbance  occur  while  it  performs  a preplanned  operation. 
The  study  reported  in  this  thesis  can  be  summarized  as 
follows . 

1.  An  analysis  of  a soft  contact  with  friction  is 
developed  in  chapter  2.  In  this  analysis,  ideal  independent 
linear  springs  and  angular  springs  were  considered.  The 
stiffness  of  the  virtual  spring  system  which  combines  these 
two  ideal  springs,  and  the  relationship  between  the  moment  and 
the  normal  force  at  the  contact  were  derived. 

2.  A procedure  for  finding  the  optimal  configuration  of 
the  grasp  based  on  the  previous  analysis  is  presented  in 
chapter  3 . The  optimal  grip  is  also  characterized  by  a 
gripping  ratio  which  is  the  required  normal  force  ratio 
between  the  fingers  within  the  friction  constraints  when  the 
gripper  grasps  an  object  in  its  initial  weightless  equilibrium 
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state  . Since  the  end  effector  does  not  produce  any  net  force 
on  the  object  , it  is  always  in  an  equilibrium  state  whether 
or  not  there  is  an  external  force.  This  can  be  determined  by 
a linear  programming  optimization  in  a very  short  (c.p.u)time. 
If  the  manipulator  is  to  perform  a preplanned  operation,  the 
optimal  configuration  which  requires  the  minimum  normal  forces 
at  the  fingertips  can  be  found  by  an  optimization  procedure 
considering  all  the  expected  force  and  moment  through  out  the 
movement  including  any  inertial  effects.  The  manipulator  can 
perform  the  preplanned  operation  successfully  by  this  optimal 
configuration  which  is  found  before  starting  the  operation  if 
there  is  no  unexpected  disturbance  during  the  operation. 

3.  In  order  to  deal  with  any  unexpected  disturbance, 
identification  of  the  disturbance  is  needed  first  to  resist  it 
effectively.  This  identification  process  with  an  optimal 
grasp  is  discussed  in  chapter  4.  Several  computational 
algorithms  are  introduced  based  on  two  types  of  sensor 
systems.  One  is  a comprehensive  sensor  system  which  can  sense 
the  direction  and  magnitude  of  all  the  frictional  and  normal 
forces  at  the  fingertips.  The  other  sensor  system  utilizes 
normal  force  transducers  only  at  the  fingertips.  An 
extrapolation  procedure  is  introduced  to  anticipate  the  next 
change  of  the  disturbance  when  the  comprehensive  sensor  system 
is  used.  A Heuristic  approach  utilizing  the  information  from 
the  system  which  has  normal  force  sensors  only  is  introduced. 
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4.  Several  control  algorithms  which  can  sustain  the 
stability  of  the  object  when  an  unexpected  disturbance  is 
detected  during  the  operation  are  developed  in  chapter  5.  The 
proposed  corrective  actions  are  implemented  by  changing  the 
required  normal  forces  and  changing  the  orientation  of  the 
object.  These  algorithms  are  relatively  simple  and  are  easy  to 
incorporate  in  a real  time  control.  The  algorithms  are 
developed  with  consideration  of  all  the  possible  events  which 
may  occur  during  the  operation. 

5.  To  examine  the  feasibility  and  limitations  of  these 
control  algorithms,  some  illustrative  computer  simulations 
were  performed  for  several  selected  circumstances  and  the 
results  are  presented  in  chapter  6. 

7.2  Conclusion 

The  following  can  be  concluded  from  the  results  of  this 
study . 

1.  If  the  manipulator  has  a comprehensive  sensor  system 
as  described  in  chapter  5,  it  can  perform  a preplanned 
operation  successfully  by  the  control  algorithms  developed  in 
this  study  if  the  unexpected  disturbances  are  manageable 
within  the  limits  imposed  on  the  maximum  normal  forces. 

2.  If  the  manipulator  has  normal  force  sensors  only,  the 
required  normal  forces  are  usually  higher  in  comparison  with 
the  case  where  a comprehensive  sensor  system  is  used. 
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3.  If  the  end  effector  has  normal  force  sensors  only, 
the  approach  proposed  in  chapter  5 for  utilizing  the  sensed 
normal  force  information  for  the  next  step  during  the 
identification  appears  to  be  feasible  in  the  selected 
examples . 

4.  The  computation  time(  c.p.u  time)  is  one  of  the 
critical  factors  in  the  control  system.  If  it  is  necessary  to 
rotate  the  object  more  than  10  times  in  the  unit  degree  about 
its  axes,  the  computational  time  would  be  more  than  50  % of 
the  0.01  sec  time  needed  for  data  acquisition  in  the  computer 
simulation . 

5.  In  the  geometric  analysis  of  the  manipulators  arm 
movement,  the  computation  is  assumed  to  be  completed  within 
the  data  acquisition  time  and  the  actuators  are  assumed  to 
respond  fast  enough  to  perform  the  task  of  changing  the 
orientations . 


7.3  Recommendations 

The  followings  are  recommendations  for  future 
investigations . 

1.  Special  attention  should  be  given  to  minimizing  the 
computation  time  by  the  design  of  a dedicated  micro  processor 
which  is  optimized  to  meet  the  special  requirement  of  the 


proposed  system. 
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2 . The  reaction  time  of  the  integrated  sensor  - control 
- actuator  system  should  be  minimized  in  order  to  meet  the 
demanding  requirements  of  responding  to  fast  occurring 
unexpected  disturbances. 


APPENDIX  A 

TRANSFORMATION  MATRICES 


Given  a force  and  moment  vector  acting  at  the  origin  of 
some  coordinate  frame  attached  to  a fixed  object,  the 
objective  is  to  find  force  and  moment  vector  acting  at  some 
other  coordinate  frame  which  is  also  attached  rigidly  to  the 
object.  A generalized  transformation  matrix  can  be  derived 
for  transforming  forces  and  moments  between  the  coordinate 
frames  when  the  position  vector  between  the  two  frames  is 
known . 

A transformation  matrix  for  a contact  point  is  defined  as; 


[J2]  [0] 

[P  ] [i?] 


( A.  1 ) 


x mx  nx 

lzy~lyz  mzy-myz  nzy-nyz 

R = 

ly  my  ny 

, p = 

1xz~1zx 

Z ^Zm 

lyX-lxy  ntyX-m xy  nyX-nxy 

Where  R is  a rotation  matrix.  The  given  position  vector 

is  [ x , y , z ] f.  As  an  example  in  order  to  derive  the 

transformation  matrix  from  frame  B to  frame  C(Figure  A.l), 
Fb  = [10,  0,  0,  0,  100,  0 ]fc  is  the  force  and  moment 
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vector  acting  at  the  origin  of  frame  B,  the  equivalent  force 
and  moment  vector  in  frame  C can  be  formulated  as  follows; 

If  R = l J • L Rv  J , position  vector  P = [ 10,  5,  10  ] t then 

'*90  >90 

Fc  = T 1 Fb  = [ 0,  0,  10,  100,  50,  0 ] c . 

From  these  transformation  matrices,  constraints  matrices 
can  be  produced  as  described  in  chapter  3 . 
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Figure  A.l  Equivalent  Force  and  Moment 


APPENDIX  B 

POLYNOMIAL  APPROXIMATION 

The  third-order  approximating  polynomial  can  be  written 

as  : 

f = aa  + a2  X + a2  X2  + a3  X3  (B.l) 

In  the  following  equations  for  the  coefficients  a0  , a1 
, a2  and  a2  , it  should  be  remembered  that  the  ordering  of  the 
point  X1  , X2  , X2  and  Xi  is  arbitrary  so  that,  for  example,. X2 
may  be  less  than  X1  . Also,  it  is  not  required  that  the  points 
be  equally  spaced. 

Two-point  linear  approximation 

Given  ; ( Xx , f1),  ( X2  , f2  ) 


& 

u> 

II 

0 

( B . 2 ) 

^2 

0 

( B . 3 ) 

II 
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- fl 

( B . 4 ) 

1 *2 
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ai  x± 

( B . 5 ) 
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Three-point  Quadratic  approximation 

Given  ; { Xx  , f x ) , ( X2 , f2  ) , ( X3  , f3  ) 


a3  = 0 


(B.6) 


(f3  ~ fx)  (f2  - fx  ) 


a2 


(X3  -Xx)  (X2  - xx) 

X3  - X2 


( B . 7 ) 


^1  y _ y 32  (-^1  + ) 

a2  a1 


( B . 8 ) 


a0  ■%! 


( B . 9 ) 


Four-point  Cubic  approximation 

Given  ; ( Xx  , fx  ) , ( X2 , f2  ) , ( X3 , f3  ) and  ( X4  , f 4 ) 


For  convenience,  define  the  following  terms: 

Qx  = X,3  U2-X3)  - x22(X3  -Xx)  + Xx3(X3  -X2)  (B.10) 

02  = X43  (X2  -Xx)  - x2  (X4  -Xx)  + Xx3(Xi  - X2 ) (B.ll) 

/ 

C>3  = (X3  -x2)  (X2  -x3)  (X3  -Xx)  ( B . 12  ) 

04  = (X4  -x2)  (X2  -Xx)  (X4  -x3)  ( B . 13  ) 

Q5  = f3  (X2  - Xx)  - f2  (X3  - Xx)  + fx  (X3  - X2)  ( B . 14 ) 

05  = f4  (X2  - Xx)  - f2  (X4  - X3)  + f3  (X4  - X2)  ( B . 15 ) 
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In  terms  of  Q1  through  Q6  , the  coefficients  now  become 


a 


3 


Qj  Q$  @4 

Q?_  Oj  ~ Q\  Qi 


( B . 16 ) 


( B . 17 ) 


f - f X 2 _ v-  3 

1 - a3  — — - a (v  +X2)  ( B . 18 ) 

z2  -X,  3 x2  -xx  2 1 

a0  = fx  - a,  X,  - a2  Z,2  - a3  V (B.19) 


APPENDIX  C 

MAIN  COMPUTRE  PROGRAM 


/**  for  cube  =>  rotate  1 deg  : Normal  force  sdensors  only**/ 

#include  <math.h> 

#include  <stdio.h> 

#include  "extra. h" 

#define  sq(x)  x*x 
#def ine  tri (x)  x*x*x 
#define  e 0.000000001 


double  tfl [6]  [6] ( tf2  [6]  [6] , tf3 [6]  [6]  , ftl [6]  [6]  , ft2 [6]  [6]  , 
f t3  [ 6 ] [ 6 ] , v, mu, dx, dy , dz , r , w, pi , rad, f 2x  [ 3 ] , f 2y  [ 3 ] , 
f 2 z [ 3 ] , m2  z [ 3 ] , bmax, bmin, f_ratio [ 3 ] [ 6 ] ; 
int  *lpBasis , 1 ; 

void  main ( ) 

{ 

double  f_unexp [ 6 ] , t ; 

int  i , n , itr , j ; 

double  rl  [3  ] [3]  ,r2  [3]  [3]  ,r3  [3]  [3],ry[3]  [3]  ,rz[3]  [3], 

rx[3]  [3  ] , al  [3  ] [3],a2[3]  [3],a3[3]  [3  ] , pi  [3  ] , p2  [3  ] , 
p3 [3 ] , fmax, Nmax, checkll , checkl2 , check21 , check22 , 
check3 1 , check32 , x [ 3 ] [3] , ml [27 ] [42] , soil  [ 42 ] , 
rhsl [27] , cost 1 [ 42 ] , ob j 1 , f lx [ 3 ] , f ly [ 3 ] , f lz [ 3 ] , 
mlz [3 ] , f zmax, m2 [29] [44] , sol2 [44] , rhs2 [29] ,cost2[44] , 
ob j 2 , bll , bl2 , b21 , b22 , f io [ 6 ] , b3 1 , b32 , 
ff [3] [6] , f fo [3 ] [6] , cn, cf , f i [ 6 ] , ro [ 3 ] [3] ,rcu[3] [3] , 
fmin, ratio, 13 [3] [3], del_f 1 , del_f 2 , del_f 3 , 
f zstart , txc , tyc , tzc , tx, ty , tz , fminx, fminy , fminz , 
txo , tyo , tzo , f exo [ 6 ] , f exto [ 6 ] , thx, thy, thz , 
tsl, tfil,wl, fel, f 1 , periodl , ts2  > tf i2 , w2 , fe2, f 2 , 
f_etral  [4]  , f_toto  [6]  , f_etra2  [4]  , f_etra3  [4]  , 
period2 ,ts3,tfi3,w3, fe3, f 3 , f fmax, f osensed, period3 , 
f ie [ 6 ] , f_tot [ 6 ] , f ex, f_ext [ 6 ] , f_sensed, 
f org, del_t , count , f_ref , sensed; 

double  extra () ; 

int  nrow, ncol , error, itre, trakye, itpol ; 

void  golp ( ) ; void  stepl ( ) ; void  step2 ( ) ; void  step3 ( ) ; 


FILE  *fpl , *fp2 , *fp3 , *fopen ( ) ; 


/ * ★ 

fpl  = f open ( " casrti5 . dl 
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fp2  = f open ( " casrti5 . d2 " , "w" ) ; 

fprintf ( fpl expected  Disturb. \n\n  t fx  fy 


12  6 


fprintf ( fp2 , 
fprint  f ( fp3 , 


★ * j 


f z mx  my  mz 
\n\n" ) ; 

"TOTAL  Disturb. \n\n  t fx  fy  fz  mx  my  mz  \n\n"  ) ; 
"Time  thetax  thetay  thetaz  forg 

Nocor .ratio \n\n" ) ; 


/ 


★ 


★ 


/ 


fp3  = f open ( "Normall . d3 " , "w" ) ; 
/**  Variables  **/ 


pi[0]==>  x position  of  i_th  finger 
pi [1] ==>  y position  of  i_th  finger 
pi [2] ==>  z position  of  i_th  finger 


f ix [ j ] ===> 

force 
step . 

of 

X 

direction 

of 

j_th 

finger 

of 

i_th 

f iy [ j ] ===> 

force 
step . 

of 

y 

direction 

of 

j_th 

finger 

of 

i_th 

f iz  [ j ] = = = > 

force 

of 

z 

direction 

of 

j_th 

finger 

of 

i_th 

step . 

ri  [ ] []  ==>  rotation  matrix  of  i_th  finger  from  OBJ  to 

contact  center. 

tfi[] []=  = > Transformation  matrix  of  i_th  finger  from  OBJ 

to  finger. 

fti[]  []=  = > Transformation  matrix  of  i_th  finger  from 

finger  to  OBJ. 

t ==>  Current  Time  from  the  start  of  the  operation 

del_t  ==>  Interval  between  the  iterations  ==  0.01  sec 
f _exp [ ] ==>  Expected  disturbance  vector  on  the  object 

system 

f_unexp[]==>  Unexpected  disturbance  vector  on  the  object 
f_tot [ ] ==>  Total  disturbance  ( f _exp  + f_unexp) 
fz[i]  ==>  Sensed  normal  force  from  i_th  fingertip 
mz  [ ] = = > Senced  moment  From  the  sensor  system  refered 

Obj  . 

f i [ ] ==>  Local  wrench  of  the  i_th  finger 


/**  Define  pi  the  modulus  of  a circle  **/ 
pi  = (double)  acos(-l.O); 

/**  Size  of  the  object  **/ 

dx  = 0.1;  dy  = 0.1;  dz  = 0.1; 

Nmax  = 0.0; 

/**  Size  of  the  Contact  Area  **/ 
mu  = 0.3; 

rad=  sqrt (0.0001/  (double)  acos(-l.O));  /**Radi.  of  the 

contact  area**/ 

v=3 . 0/ (2 . 0*pi* (rad) *mu) ; /**  Propotionality  const.  **/ 

/**  Pre  planned  Finger  Position  ==>  Input  Data**/ 
x [ 0 ] [0]=-dx/2.0;x[0]  [ 1 ] = 0 . 0 ; x[0]  [2]=0.0; 
x[l]  [0] =0 .00392950241;  x[l]  [l]=-dy/2.0; 
x [ 1 ] [2] =0 .0142992626; 

x [2 ] [ 0 ] =dx/ 2.0;  x[2][l]=0.0;  x[2][2]=0.0; 


pi [ 0 ] =x [ 0 ] [ 0 ] ;pl [ 1] =x [ 0 ] [ 1 ] ; pi [2 ] =x [ 0 ] [2  ] ; 
p2[0]=x[l]  [0]  ;p2[l]=x[l]  [1]  ;p2[2]=x[l]  [2]  ; 
p3 [0]=x[2]  [0] ;p3 [l]=x[2]  [1] ;p3 [2]=x[2]  [2] ; 

for  ( i = 0 ; i < 3 ; i ++  ) 
for  ( j = 0 ; j < 3 ; j ++  ) 

{ 13 [i] [j]  = 0.0  ; 

if ( i ==  j ) 

13 [i]  [j]  =1.0  ; 

} 

Make  the  rotation  matrices  for  each  finger  points 


rotate ( rx, -90 . 0 , ' x ' ) 
rotate ( rz , -90 . 0 , ' z ' ) 
mult 3 (rl,rx,rz) ; 
rotate ( ry , 90 . 0 , 'y ' ) ; 
rotate ( rz , 90 . 0 , ' z ' ) ; 
mult 3 ( r2 , ry , rz ) ; 
rotate (rx, -90 . 0, 'x') 
rotate ( rz , 90 . 0 , ' z ' ) ; 
mult 3 ( r3 , rx, rz ) ; 
mult (pi , rl , pi ) ; 
mult (p2 , r2 , p2 ) ; 
mult (p3 , r3 , p3 ) ; 
f or ( i=0 ; i<3 ; i++ ) 

{ pi [i] =-pl [i] ; 
p2 [i] =-p2 [i]  ; 
p3 [i] =-p3  [i] ; 

} 

al [0] [0] =pl [1] *rl [2] 
al [1] [0] =pl [2] *rl [0] 
al  [2]  [0]  =pl  [0]  *rl  [1] 
al [ 0 ] [1] =pl [1] *rl [2] 
al [ 1 ] [ 1 ] =pl [2 ] *rl [ 0 ] 
al [ 2 ] [l]=pl[0] *rl[l] 
al [ 0 ] [ 2 ] =pl [ 1 ] *rl [2 ] 
al [ 1 ] [2]=pl[2]*rl[0] 
al [ 2 ] [ 2 ] =pl [ 0 ] *rl [ 1 ] 
a2  [0]  [0]  =p2  [1]  *r2  [2] 
a2  [1]  [0]  =p2  [2]  *r2  [0] 
a2  [2]  [0]  =p2  [0]  *r2  [1] 
a2  [0]  [1]  =p2  [1]  *r2  [2] 
a2  [1]  [1]  =p2  [2]  *r2  [0] 
a2  [2]  [1]  =p2  [0]  *r2  [1] 
a2  [0]  [2]  =p2  [1]  *r2  [2] 
a2  [1]  [2]  =p2  [2]  *r2  [0] 
a2  [2]  [2]  =p2  [0]  *r2  [1] 
a3  [0]  [0]  =p3  [1]  *r3  [2] 
a3  [1]  [0]  =p3  [2] *r3  [0] 
a3  [2]  [0]  =p3  [0]  *r3  [1] 
a3  [0]  [1]  =p3  [1]  *r3  [2] 


[0] 

[0] 

[0] 

[1] 

[1] 

[1] 

[2] 

[2] 

[2] 

[0] 

[0] 

[0] 

[1] 

[1] 

[1] 

[2] 

[2] 

[2] 

[0] 

[0] 

[0] 

[1] 


■pl  [2] 
■pi  [0] 
■pl  [1] 
■pl  C2  ] 
■pl  CO] 
pl  [ 1 3 
■pl  [2] 

pl  [ 0 ] 

■pl  [13 

•p2  [2] 
•p2  [0] 
■p2  [1] 
■p2  [2] 
■p2  [0] 
•p2  [1] 
•p2  [2] 
p2  [0] 
■p2  [1] 

■p3  [2] 
■p3  [0] 
■p3  [1] 
■p3  [2] 


*rl  [1] 
*rl [2] 
*rl [ 0 ] 
*rl [1] 
*rl [2 ] 
*rl [0] 
*rl [1] 
*rl [2] 
*rl [0] 
*r2 [1] 
*r2  [2] 
*r2 [0] 
*r2 [1] 
*r2  [2] 
*r2  [0] 
*r2  [1] 
*r2 [2] 
*r2 [ 0 ] 
*r3 [1] 
*r3 [2] 
*r3 [0] 
*r3 [1] 


[0] 
[0] 
[0] 
[1] 
[1] 
[1] 
[2] 
[2]  , 
[2] 
[0] 
[0] 
[0] 
[1] 
[1] 
[1] 
[2] 
[2] 
[2] 
[0] 
[0] 
[0] 
[1] 


a3[l] [ 1 ] =p3 [2 ] *r3 [ 0 ] [1] -p3 [0] *r3 [2 ] [1] 
a3[2]  [I]=p3[0]*r3[l]  [ 1 ] -p3  [ 1 ] *r3  [ 0 ] [1] 
a3  [0]  [2]  =p3  [1]  *r3  [2]  [2  ] -p3  [2  ] * r 3 [1  ] [2] 
a3 [1]  [2] =p3 [2] *r3 [0]  [2 ] -p3 [0] *r3 [2 ] [2] 
a 3 [ 2 ] [2]  =p3  [0]  *r3  [1]  [2] -p3  [1]  *r3  [0]  [2] 

f or ( i=0 ; i<6 ; i++ ) 

for ( j =0 ; j<6 ; j ++ ) 

{ ftl[i]  [ j ] = 0 . 0 ; 
ft2 [i]  [ j ] = 0 . 0 ; 
ft3  [i]  [ j ] = 0 . 0 ; 

} 

for ( i=0 ; i<3 ; i++ ) 
for ( j =0 ; j<3 ; j ++ ) 

{ ftl[i]  [j]=rl[i]  [j]  ; 

f tl  [ i + 3 ] [ j+3] =rl [i]  [j] ; 
f tl  [i  + 3 ] [j]=al[i]  [j]  ; 

} 

f or ( i = 0 ; i<  3 ; i ++ ) 
for ( j=0; j<3; j++) 

{ f 1 2 [ i ] [ j ] =r2  [ i ] [j]  ; 

f t2 [ i+3 ] [j+3] =r2 [i] [j] ; 
f t2 [ i+3 ] [ j ] =a2 [i] [j] ; 

} 

for ( i=0 ; i<3 ; i++ ) 

for ( j =0 ; j<3 ; j++) 

{ f 1 3 [ i ] [ j ] = r 3 [ i ] [j]; 

ft3 [i  + 3]  [ j +3 ] =r3  [i]  [j]  ; 
ft3 [i  + 3]  [ j ] =a3  [i]  [j]  ; 

} 

f or ( i=0 ; i<6 ; i++ ) 
for ( j =0 ; j<6 ; j ++ ) 

{ tfl[i]  [ j ] = 0 . 0 ; 
tf 2 [i]  [ j ] = 0 . 0 ; 
tf 3 [i]  [ j ] = 0 . 0 ; 

} 

f or ( i=0 ; i<3 ; i++ ) 

for ( j=0 ; j <3 ; j++) 

{ tfl[i] [ j ] =rl [ j ] [i] ; 
tf 1 [i+3 ] [j+3] =rl [ j ] [i]  ; 
tf 1 [i+3 ] [ j ] =al  [ j ] [i]  ; 

} 

f or ( i=0 ; i<3 ; i++ ) 
for ( j =0 ; j<3 ; j++) 

{ tf 2 [i] [ j ] =r2 [j] [i] ; 
t f 2 [i  + 3]  [ j +3 ] =r2  [j]  [i]  ; 
tf  2 [ i + 3 ] [j]=a2[j]  [i]  ; 

} 

for ( i=0 ; i<3 ; i++ ) 
for ( j =0 ; j <3 ; j ++ ) 

{ tf 3 [i] [ j ] =r3 [j] [i] ; 
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tf 3 [i+3]  [ j +3 ] =r3  [j]  [i] ; 
tf 3 [i  + 3]  [ j ] =a3 [j]  [i]  ; 

} 

/**  Find  the  first  grasp  ratio  **/ 
stepl ( v, tf 1 , tf 2 , t f 3 , krhsl ,&costl, &ml ) ; 
nrow  = 25;  ncol  = 42; 


golp  ( 

if  ( ! 

flx[0] 
fly [0] 
flz [0] 
ml z [ 0 ] 
flx[l] 

fly [l] 
flz[l] 
ml z [ 1 ] 
f lx [2 ] 
fly [2] 
flz [2] 
ml z [ 2 ] 
print  f ( 


nrow,  ncol,  ml,  costl,  rhsl,&soll,  &objl,  terror) 
error  ) { 


soli [ 0 ] -soil  [ 1 ] ; 
soil [2] -soli  [3]  ; 
soil  [4 ] ; 

soli [5] -soil [6] ; 
soli [7] -soli [8] ; 
soli [ 9 ] -soli [ 10 ] ; 
soli [ 11 ] ; 
soil [12] -soil [13] 
soil [14] -soli [15] 
soil [16] -soil [17] 
soil  [18 ] ; 
soil [19] -soli [20] 


step  1 : fmax  = 


p r i n t f ( 
p r i n t f ( 
p r i n t f ( 


f lx  = 


f 2 x = 


f 3 x = 


f 

\n 

f 

\n 

f 

\n 


f Mmax 
%f  \n" 
f ly  = 
f lx [ 0 ] 
f 2 y = 
flx[l] 
f 3y  = 
f lx [ 2 ] 


= % f OBJ  = 

soil [21] , soil [22] , objl) 
; f f 1 z = % f ml  z = % 

fly [0] , flz [0] ,mlz [0] ) ; 

; f f 2 z = % f m2z  = % 

f ly  [ 1 ] , f 1 z [ 1 ] , ml  z [ 1 ] ) ; 

; f f 3 z = % f m3z  = % 

f ly  [ 2 ] , f 1 z [ 2 ] , ml  z [ 2 ] ) ; 


} 

else  { 

Nmax  = 100000000; 
goto  step6; 

} 


/*****  Check  the  frictional  constraints  for  each  finger  ***/ 
checkll  = (double) sqrt ( fix [ 0 ]* fix [ 0 ] +fly [ 0 ]* fly [ 0 ]) ; 
checkl2  = v*dabs (mlz [ 0 ] ) ; 

check21  = (double) sqrt ( fix [1] *f lx [ 1 ] +fly [1 ] *fly [ 1 ]) ; 
check22  = v*dabs (mlz [ 1 ] ) ; 

check31  = (double) sqrt ( f lx [2 ] *f lx [2 ] +f lx [2 ] *f lx [2 ] ) ; 
check32  = v*dabs (mlz [ 2 ] ) ; 

if ( (mu*flz[0])  < checkll  II  (mu*flz[0])  < checkl2  ) 

{ 

f or ( i=0 ; i<3 ; i++ ) 

{ f lx [ i ] =0 . 0 ; 
f ly  [ i ] = 0 . 0 ; 
flz [i] =0 .0; 
ml z [ i ] = 0 . 0 ; } 

goto  step2; 

} 

if ( (mu*flz[l])  < check21  II  (mu*flz[l])  < check22  ) 
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{ 

for ( i = 0 ; i<3 ; i + + ) 

{ f lx [i] =0 . 0 ; 
f ly [ i ] =0.0; 
f 1 z [ i ] = 0 . 0 ; 
ml z [ i ] = 0 . 0 ; } 

goto  step2; 

} 

if ( (mu*flz[2])  < check31  II  (mu*flz[2])  < check32  ) 

{ 

for ( i=0 ; i<3 ; i++ ) 

{ f lx [ i] =0 . 0 ; 
f ly  [i]  =0 . 0 ; 
f 1 z [ i ] = 0 . 0 ; 
ml z [ i ] = 0 . 0 ; } 

goto  step2 ; 


/****  Calculate  the  First  Grasping  Ratio  ***/ 


I ★ ★ 


■k  -k  / 


f zmax  = f 1 z [ 0 ] ; 

i f ( f 1 z [ 1 ] > f zmax  ) f zmax  = f 1 z [ 1 ] ; 
if (flz [2]  > f zmax  ) f zmax  = f 1 z [ 2 ] ; 
f or ( i=0 ; i<3 ; i++ ) ' 

{ flx[i]  = fix [i] /f zmax; 
f ly [ i ] = f ly [ i ] / f zmax ; 
flz[i]  = f 1 z [ i ] / f zmax ; 
ml  z [ i ] = ml z [ i ] / f zmax ; 

} 

print f(“  First  ratio  = %f  \n",fzmax); 
printf  ( " f 1 x = % f f ly  = % f >flz  = %f  mlz  = %f 

\n" , f lx [ 0 ] , f ly [ 0 ] , f lz [ 0 ] , mlz [ 0 ] ) ; 
printf("  f 2 x = % f f 2 y = % f f 2 z = % f m2z  = %f 

\n" , flx[l] , fly [1] , flz [1] ,mlz [1] ) ; 
printf  ( " f 3 x = % f f 3 y = % f f 3 z = % f m3z  = %f 

\n" , f lx [2 ] , f ly [ 2 ] , f lz [2 ] , mlz [2 ] ) ; 


/****  If  any  finger  has  no  normal  force,  another  ratio  can  be 
calculated  for  which  all  fingers  are  constrainted  to  be 
active . **** / 


if ( flz[0]  ==  0.0  ) goto  step2; 
else  if ( flz[l]  ==  0.0  ) goto  step2; 
else  if ( flz  [2]  ==  0.0  ) goto  step2; 
else 

{ f or ( i=0 ; i<3 ; i++ ) 

{ f 2x [ i ] =f lx [ i ] ; 
f 2y [ i ] =f ly [ i ] ; 
f 2 z [i] =f lz  [i]  ; 
m2  z [ i ] =ml  z [ i ] ; 

} 
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} 


goto  step23 


step2 : 

step2 (v,tfl,tf2,tf3, &rhs2 , &cost2 , &m2 ) 
nrow  = 27;  ncol  = 44; 
golp ( nrow,  ncol,  m2,  cost2, 

if  ( 

{ 


rhs2,&sol2,  &obj2,  terror) 


! error 

) 

f 2x [ 0 ] 

— 

sol2 

[0] 

— 

sol2 [ 1] ; 

f 2y [ 0 ] 

— 

sol2 

[2] 

— 

sol2  [3]  ; 

f 2z  [ 0 ] 

— 

sol2 

[4] 

• 

/ 

m2  z [ 0 ] 

— 

sol2 

[5] 

— 

SOl2 [6] ; 

f 2x [ 1 ] 

— 

sol2 

[7] 

— 

sol2  [8]  ; 

f 2y [ 1 ] 

— 

sol2 

[9] 

— 

sol2  [10]  ; 

f2z [1] 

— 

sol2 

[11 

] 

• 

f 

m2  z [ 1 ] 

— 

sol2 

[12 

] 

-sol2  [13]  ; 

f 2x [ 2 ] 

— 

sol2 

[14 

] 

-sol2  [15]  ; 

f2y [2] 

— 

sol2 

[16 

] 

-Sol2  [17]  ; 

f2z  [2] 

— 

sol2 

[18 

] 

• 

f 

m2  z [2] 

— 

sol2 

[19 

] 

-sol2  [20]  ; 

\ 

Nmax  = 

100000000; 

goto  step6; 

} 

else 
{ 

} 

step23 : 

bmax  = f 2 z [ 0 ] ; 
ford  = 1;  i < 
if ( bmax 


bmin  = 1 
3;  i++  ) 
< f2z [i] 


0 


) bmax  = f 2 z [ i 1 


/***  Adjust  the  normal  forces  so  that  fingers  can  provide  the 
required  frictional  forces  at  the  moment  ***/ 


checkll  = ( double ) sqrt ( sq ( f2x [ 0 ]) +sq ( f2y  [ 0 ]))  ; 

checkl2  = v*dabs ( sol2 [ 5 ] -sol2 [ 6 ] ) ; 

check21  = ( double ) sqrt ( sq ( f2x [ 1 ]) +sq ( f2y [ 1 ]))  ; 

check22  = v*dabs ( sol2 [ 12 ] -sol2 [ 13  ] ) ; 

check31  = ( double) sqrt ( sq ( f 2x [2 ] ) +sq ( f 2y [ 2 ] ) ) ; 

check32  = v*dabs ( sol2 [ 19 ] -sol2 [2 0 ] ) ; 

bll  = checkll/ (mu*f2z [ 0 ])  ; 

b21  = check21/ (mu*f2z [1] ) ; 

b31  = check31/ (mu*f 2z [2  ] ) ; 

bl2  = checkl2 / (mu* f 2 z [ 0 ] ) ; 

b22  = check22 / (mu* f 2 z [ 1 ] ) ; 

b32  = check32 / (mu* f 2 z [ 2 ] ) ; 

bmax  = bll; 

if ( bmax  < bl2  ) bmax  = bl2 ; 

if ( bmax  < b21  ) bmax  = b21; 

if ( bmax  < b22  ) bmax  = bl2  ; 

if ( bmax  < b31  ) bmax  = b31; 

if ( bmax  < b32  ) bmax  = b32; 

/****  Calculate  the  Second  Grasping  Ratio  ***/ 
f or ( i=0 ; i<3 ; i++ ) 
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/ 


{ f 2x [ i ] 
f 2y [ i ] 
f2z [i] 
m2  z [ i ] 


f lx [ i ] + ( f 2x [ i ] -f lx [ i ] ) /bmax; 
f ly [ i ] + ( f 2y [ i ] -f ly [ i ] ) /bmax; 
flz [i]  + ( f 2 z [i] -flz [i] ) /bmax; 
ml z [ i ] + ( m2  z [ i ] -ml z [ i ] ) / bmax ; 


} 

for(i  = 0;  i < 3 ; i++  ) 

if (f2z  [i]  < bmin)  { bmin  = f2z[i];  l = i; 


} 


printf ( " Second  ratio  = %f  \n",bmin); 
print  f ( " f 1 x = % f f 1 y = % f f 1 z = % f m 1 z = % f 

\n",f2x[0] ,f2y[0] , f2z [0] ,m2z [0] ) ; 
printf("  f 2 x = % f f 2 y = % f f 2 z = % f m2  z = % f 

\n" , f 2x [ 1 ] , f2y [1] , f2z [1] ,m2z [1] ) ; 
printf  ( " f 3 x = % f f 3 y = % f f 3 z = % f m3z  = %f 

\n" , f 2x  [2  ] , f 2y  f 2 ] , f2z  [2  ] ,m2z[2]  ) ; 


checkll  = ( double) sqrt ( f2x [ 0 ]* f2x [ 0 ] +f2y [ 0 ]* f2y [ 0 ]) ; 
checkl2  = v*dabs (m2z [0] ) ; 

check21  = (double) sqrt ( f2x [1] *f2x [1] +f2y [1] *f2y [1] ) ; 
check22  = v*dabs (m2z [1] ) ; 

check31  = (double) sqrt ( f 2x [2 ] *f 2x [2 ] +f 2y [2 ] *f 2y [2 ] ) ; 
check32  = v*dabs (m2 z [ 2 ] ) ; 

if ( mu*f2z[0]  < checkll  &&  dabs (mu* f 2 z [ 0 ] - checkll)  > e) 
{ Nmax  = 100000000; 

goto  step6; 

} 

if  ( mu*f2z[0]  < checkl2  &&  dabs (mu* f 2 z [ 0 ] -checkl2 ) > e ) 
{ Nmax  = 100000000; 

goto  step6; 

} 

if ( mu*f2z[l]  < check21  &&  dabs (mu* f 2 z [ 1 ] - check21)  > e) 
{ Nmax  = 100000000; 

goto  step6; 

} 

if  ( mu*f2z[l]  < check22  &&  dabs (mu* f 2 z [ 1 ] -check22 ) > e ) 
{ Nmax  = 100000000; 

goto  step6; 

} 

if ( mu*f2z[2]  < check31  &&  dabs (mu* f 2 z [ 2 ] - check31)  > e) 
{ Nmax  = 100000000; 

goto  step6; 

} 

if  ( mu*f2z[2]  < check32  &&  dabs (mu* f 2 z [ 2 ] -check32 ) > e ) 
{ Nmax  = 100000000; 

goto  step6; 

} 


/**  Initial  orientation  of  the  object  **/ 

for  ( i = 0 ; i < 3 ; i ++  ) 
for  ( j = 0 ; j < 3 ; j ++  ) 
rcu [ i ] [ j ] = 13  [i]  [j ] ; 
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/**  Input  Given  Expected  Disturbance  at  this  moment  **/ 


j ~k  -k 


k k j 


ratio  = 0.0; 
t = 0.0; 
itr  = 0; 

tsl  = 0.21;  /**  start  the  first  disturbance  **/ 

ts2  = 0.51;  /**  start  the  second  disturbance  **/ 

ts3  = 1.21;  /**  start  the  third  disturbance  **/ 

tfil  = 0.31;/**  start  to  reduce  the  first  disturb.**/ 

tfi2  = 0.71;/**  start  to  reduce  the  second  disturb.**/ 

tfi3  = 1.41;/**  start  to  reduce  the  third  disturb.**/ 

fl  = 10.0;/**  Max.  of  the  first  disturb.**/ 

f2  = -0.35;/**  Max.  of  the  ssecond  disturb.**/ 

f3  = -0.2;  /**  Max.  of  the  third  disturb.  **/ 

periodl  = 0.4  ; 

period2  = 0.4  ; 

period3  = 0.4  ; 

wl  = 2 . 0*pi/periodl ; 

w2  = 2 . 0*pi/period2 ; 

w3  = 2 . 0*pi/period3 ; 

del_t  =0.01; 

forg  = ffmax=0.0; 

thx  = thy  = thz  = 0.0; 

tx=ty=tz=txc=tyc=tzc=0 . 0 ; 

while  ( t < 1.7  ) 

{ 

t = itr*del_t; 

printf ( " \n\nTime  T = %f\n“,t); 

fort  j = 0 ; j < 6 ; j++  ) 
f i [ j ] = 0.0  ; 

printf ( "fx=%f  fy=%f  fz=%f  mx=%f  my=%f 

mz=%f \n" , fi  [0] ,fi[l],fi[2],fi[3],fi[4],fi[5]); 

fprintf ( fpl,  " %f  %f  %f  %f  %f  %f  %f \n " , t , f i [ 0 ] , f i [ 1 ] , 

f i [2  ] , f i [3  ] , f i [4 ] , f i [5]  ) ; 


for  ( i = 0 ; i<6  ; i++  ) 
f ie [i]  = 0.0  ; 

fel  = fe2  = fe3  = fex  = 0.0; 

if  ( t > tsl  &&  t <=  ( tsl+periodl/4 . 0 )) 

{ fel  = f l*sin (wl* ( t-tsl ) ) ; 
trakye  = 1; 

} 

else  if  ( t > ( tsl+periodl/4 . 0 ) &&  t <=  tfil  ) 
fel  = fl; 

else  if  ( t > tfil  &&  t < tf il+periodl/4 . 0 ) 
fel  = f l*cos (wl* ( t-tf il ) ) ; 
else  if  ( t >=  ( tf il+periodl/4 . 0 ) &&  t <=  ts2  ) 
{ fel  = 0.0; 
trakye  = 0; 

} 

if  ( t > ts2  &&  t <=  ( ts2+period2 /4 . 0 )) 
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{ fe2  = f2*sin(w2*(t-ts2)); 
trakye  = 2 ; 

} 

else  if  ( t > ( ts2+period2 /4 . 0 ) &&  t <=  tfi2  ) 
fe2  = f 2 ; 

else  if  ( t > tfi2  &&  t < tf i2+period2 /4 . 0 ) 
fe2  = f2*cos (w2* (t-tf i2 ) ) ; 
else  if  ( t >=  ( t f i2+period2 /4 . 0 ) &&  t < ts3  ) 

{ fe2  = 0.0; 
trakye  = 0; 

} 

if  ( t > ts3  &&  t <=  ( ts3+period3 /4 . 0 )) 

{ fe3  = f 3*sin (w3 * ( t-ts3 ) ) ; 
trakye  = 3 ; 

} 

else  if  ( t > ( ts3+period3 /4 . 0 ) &&  t <=  tfi3  ) 
fe3  = f 3 ; 

else  if  ( t > tfi3  &&  t < tf i3 +period3 /4 . 0 ) 
fe3  = f 3*cos (w3* (t-tf i3 ) ) ; 
else  if  ( t >=  ( tf i3+period3 /4 . 0 ) ) 

{ fe3  = 0.0; 
trakye  = 0; 

} 

fex  = fel+fe2+fe3; 


/**  Sumation  all  of  the  external  disturbance  **/ 


f i e [ 0 ] = 0.0; 
fie[l]  = 0.0; 
if  ( trakye  ==  1 ) 
fie[2]  = fex; 
if  ( trakye  ==  2 ) 
f i e [ 4 ] = f ex ; 
if  ( trakye  ==  3 ) 
f i e [ 5 ] = f ex  ; 
f ie [3 ] = 0.0; 

printf("  %f  %f  %f  %f  %f 
%f\n" , f ie [ 0 ] , f ie [ 1 ] , f ie [2 ] , f ie [ 3 ] , 

f ie [ 4 ] , f ie [ 5 ] ) ; 


j * ★ 


★ ★ 


/ 


for  ( i = 0 ; i < 6 ; i ++  ) 

f_toto[i]  = fi[i]  + fie[i]  ; 
printf("t  = %f  Tot  fx=%f  fy=%f  fz=%f  mx=%f  my=%f 

mz=%f \n" , t , f_toto [ 0 ] , f_toto [ 1 ] , f_toto [2] , 

f_toto [ 3 ] , f_toto [ 4 ] , f_toto [ 5 ] ) ; 

fprintf(fp2,"%f  % f % f % f % f % f 

%f \n " , t , f_toto [ 0 ] , f_toto [ 1 ] , 

f_toto [ 2 ] , f _toto [ 3 ] , f_toto [ 4 ] , f _toto [ 5 ] ) ; 

for  ( i=0  ; i<3  ; i++  ) 

printf("rcu  = % f % f 
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% f \n " , rcu [ i ] [ 0 ] , rcu [ i ] [ 1 ] , rcu [ i ] [ 2 ] ) ; 


f_sensed  = 0.0; 
fosensed  = 0.0; 
sensed  = 0.0; 
itr  + = 1; 

for  ( i = 0 ; i<6  ; i++  ) 

{ f_tot [i]  = f i [i]  + fie [ i]  ; 
sensed  +=  dabs ( f_toto [i] ) ; 
f_sensed  +=  dabs ( f_tot [ i ] ) ; 
fosensed  + = dabs(fi[i]); 

} 

if  ( f_sensed  ==0.0  &&  sensed  ==  0.0  ) 

{ f or  ( i = 0 ; i < 3 ; i ++  ) 

for  ( j = 0 ; j < 3 ; j ++  ) 
rcu [ i ] [j]  =13 [i]  [j]  ; 

tx  = ty  = tz  = 0.0; 
thx  = thy  = thz  = 0.0; 
ffmax  = forg  = 0.0; 
for ( i = 0 ; i < 3 ; i ++  ) 

for  ( j = 0 ; j < 3 ; j ++  ) 
ffo [i] [j]  = 0.0; 
for  ( i = 0 ; i<6  ; i++  ) 

' fexo[i]  = f_toto[ij; 
goto  cont; 

} 

if  ( f_sensed  ==0.0  ) 

{ for  ( i = 0 ; i < 3 ; i ++  ) 

for  ( j = 0 ; j < 3 ; j ++  ) 

rcu[i] [j]  =13 [i] [j] ; 
tx  = ty  = tz  = 0.0; 
thx  = thy  = thz  = 0.0; 
ffmax  = forg  = 0.0; 
for  ( i = 0 ; i<6  ; i++  ) 
fexo[i]  = f_toto[i]; 
goto  cont  ; 

} 

if  ( fex  ==  0.0  ) 

{ f or  ( i = 0 ; i < 3 ; i ++  ) 

for  ( j = 0 ; j < 3 ; j ++  ) 

rcu [ i ] [ j ] = 1 3 [ i ] [ j ] ; 

if ( ( dabs ( f_toto [ 0 ] ) +dabs ( f_toto [ 1 ] ) + 

dabs ( f_toto [2 ] ) ) ==  0.0  ) 

f toto [ 1 ] = - 0.000000001; 

check ( f_toto, f f , &f fmax) ; 
if  ( ratio  < ffmax  ) 
ratio  = ffmax  ; 
forg  = ffmax  ; 
tx  = ty  = tz  = 0.0; 
thx  = thy  = thz  = 0.0; 

print f ("  time  = %f  tx=  %f  ty=  %f  tz=  %f  required=%f  No 

forg.=%f  \n" , t, tx, ty, tz, ffrrax, forg) ; 
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for  ( i = 0 ; i < 6 ; i ++  ) 
fexo[i]  = f_toto[i]; 
goto  cont ; 

} 

else 

{ 

if ( (dabs ( f exo [ 0 ] ) +dabs ( f exo [ 1 ] ) +dabs ( f exo [2 ] ) ) ==0 . 0 ) 
fexo [1]  = - 0.000000001; 

f _ext [ 0 ] = rcu [ 0 ] [ 0 ] * f exo [ 0 ] + 

rcu [0]  [1]  *fexo [1] +rcu  [0]  [2] *fexo [2] ; 
f _ext [1]  = r cu [ 1 ] [ 0 ] * f exo [ 0 ] + 

rcu [ 1 ] [ 1 ] * f exo [ 1] +rcu  [ 1 ] [ 2 ] * f exo [ 2 ] ; 
f_ext[2]  = rcu [2] [0]*fexo[0]+ 

rcu [2 ] [1] *fexo [1] +rcu [2 ] [2 ] *f exo [2 ] ; 
f_ext[3]  = rcu [ 0 ] [ 0 ] * f exo [ 3 ] + 

rcu [0] [1] *f exo [4] +rcu [0] [2 ] *f exo [5]  ; 
f_ext[4]  = rcu [1] [0] *fexo [3 ] + 

rcu  [1]  [1]  *fexo  [4]  +rcu  [1]  [2]  *fexo  [5]  ; 
f_ext[5]  = rcu [2 ] [ 0 ] *f exo [ 3 ] + 

rcu [2] [1] *fexo[4] +rcu[2] [2] *fexo [5] ; 
check ( f_ext , f fo, &fmax) ; 

} 

if ( (dabs ( f_toto [ 0 ] ) +dabs ( f_toto [ 1 ] ) + 

dabs ( f_toto [2 ] ) ) ==  0.0  ) 

f toto [ 1 ] = - 0.000000001; 

f _ext [ 0 ] = rcu [ 0 ] [ 0 ] * f_toto [ 0 ] +rcu [ 0 ] [1] *f_toto [1] + 

rcu [ 0 ] [ 2 ] * f _toto [ 2 ] ; 

f_ext[l]  = rcu [1] [0] *f_toto [0] +rcu [1] [1] *f_toto [1] + 

rcu [ 1 ] [ 2 ] * f _toto [ 2 ] ; 

f_ext[2]  = rcu [2 ] [ 0 ] * f_toto [ 0 ] +rcu [2 ] [ 1 ] * f_toto [ 1 ] + 

rcu [2 ] [2 ] *f_toto [2 ] ; 

f_ext[3]  = rcu [ 0 ] [ 0 ] * f_toto [ 3 ] +rcu [ 0 ] [ 1 ] * f_toto [ 4 ] + 

rcu [ 0 ] [ 2 ] * f_toto [ 5 ] ; 

f_ext[4]  = rcu [1 ] [ 0 ] *f_toto [3 ] +rcu [ 1 ] [ 1 ] *f_toto [4 ] + 

rcu [ 1 ] [ 2 ] * f_toto [ 5 ] ; 

f_ext[5]  = rcu [2 ] [ 0 ] * f_toto [ 3 ] +rcu [ 2 ] [ 1 ] * f_toto [ 4 ] + 

rcu [2 ] [2 ] *f_toto [5]  ; 

check ( f_ext , f f , &f  fmax) ; 
if  ( ffmax  > ratio  ) 
ratio  = ffmax  ; 
check  ( f_toto,  f f , Scforg)  ; 

printf ( " time=%f  required=%f  No  cor.=%f 

ra=%f \n" , t , ffmax, forg, ratio) ; 

/*Monitor  the  Normal  force  changes  for  the  next  step  */ 


next : 

i f ( ( dabs ( f _tot [ 0 ] ) +dabs ( f_tot [ 1 ] ) +dabs ( f _tot [2] ) ) ==0 . 0 ) 

f tot [ 1 ] = - 0.000000001; 

f_ext [ 0 ] = rcu [ 0 ] [ 0 ] * f_tot [ 0 ] +rcu [ 0 ] [ 1 ] * f_tot [ 1 ] + 

rcu [ 0 ] [2 ] * f_tot [ 2 ] ; 

f_ext[l]  = rcu [ 1 ] [ 0 ] * f_tot [ 0 ] +rcu [ 1 ] [ 1 ] * f_tot [ 1 ] + 
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rcu [ 1 ] 

f _ext [ 2 ] = rcu [2] [ 0 ] *f_tot [ 0 ] +rcu [2 ] [1] 

rcu [ 2 ] 

f_ext[3]  = rcu[0] [ 0 ] *f_tot [3 ] +rcu [ 0 ] [1] 

rcu [ 0 ] 

f_ext[4]  = rcu[l] [ 0 ] * f_tot [ 3 ] +rcu [ 1 ] [1] 

rcu [ 1 ] 

f_ext[5]  = rcu [2] [ 0 ] *f_tot [ 3 ] +rcu [2 ] [1] 

rcu [ 2 ] 

check ( f _ext , f f , & f max ) ; 


[2] 

*f_ 

[2] 

*f_ 

[2] 

*f_ 

[2] 

*f_ 

[2] 


* f_tot [ 2 ] 
tot [ 1 ] + 

* f_tot [ 2 ] 
tot [ 4 ] + 

* f_tot [ 5 ] 
tot  [ 4 ] + 

* f_tot [ 5 ] 
tot [ 4 ] + 

* f_tot [ 5 ] 


/ 


★ ★ 


Calculate  the  change  of  the  normal  forces  ***/ 


del_f 1 = f f [ 0 ] [ 2 ] - f fo [0] [2 ] ; 
del_f 2 = f f [ 1 ] [ 2 ] - f f o [ 1 ] [ 2 ] ; 

del_f 3 = f f [2 ] [2 ] - f f o [ 2 ] [ 2 ] ; 

for  ( i = 0 ; i < 6 ; i ++  ) 
for  ( j = 0 ; j < 3 ; j ++  ) 
ro [ i ] [ j ] = rcu [ j ] [ i ] ; 
mult 3 (ro, ro, rcu) ; 
f exo [0]  = ro [ 0 ] [ 0 ] * f _ext [ 0 ] + 

ro  [ 0 ] [ 1 ] * f _ext [ L] +ro [ 0 ] [ 2 ] * f _ext [ 2 ] ; 
fexo[l]  = ro[l] [ 0 ] *f_ext [ 0 ] + 

ro [ 1 ] [ 1 ] * f _ext [ 1 ] +ro [ 1 ] [ 2 ] * f _ext [ 2 ] ; 
f exo [2]  = r o [ 2 ] [ 0 ] * f _ext [ 0 ] + 

ro [ 0 ] [ 1 ] * f _ext [ 1 ] +ro [ 2 ] [2]*  f _ext [ 2 ] ; 
fexo[3]  = ro [0] [0] *f_ext [3] + 

ro [ 0 ] [ 1 ] * f_ext [ 4 ] +ro [ 0 ] [2]*f_ext [5] ; 
f exo [4]  = r o [ 1 ] [ 0 ] * f _ext [ 3 ] + 

ro [ 0 ] [ 1 ] * f_ext [ 4 ] +ro [ 1 ] [ 2 ] * f_ext [ 5 ] ; 
f exo [5]  = r o [ 2 ] [ 0 ] * f _ext [ 3 ] + 

ro [ 0 ] [1] *f_ext [4] +ro [2] [2] *f_ext [5] ; 

/**  Add  normal  force  changes  as  a next  disturbance  for  next**/ 

printf("dfl  = %f  df2  = %f  df3  = %f  \n", 

del_f 1 , del_f 2 , del_f 3 ) ; 
if (dabs (del_f 1) <e  &&  dabs ( del_f 2 ) <e  &&  dabs (del_f 3 ) <e ) 
{ print f ( " KEEP  the  POSITION  \n"); 
tx=ty=tz=0 . 0 ; 
txc  = tyc  = tzc  = 0.0; 
goto  cont ; 

} 

f ie [ 0 ] = tfl  [0]  [2] *del_fl  + tf2 [0]  [2] *del_f2  + 

tf3[0] [2]*del_f3; 

f ie [ 1 ] - tfl  [1]  [2] *del_fl  + tf2 [1]  [2] *del_f2  + 

tf 3 [1] [2] *del_f3 ; 

f ie [ 2 ] = tfl  [2]  [2] *del_fl+tf2 [2]  [2] *del_f2  + 

tf 3 [2]  [2]*del_f3; 

f ie [3 ] = tfl  [3]  [2] *del_fl  + tf2 [3]  [2] *del_f2  + 

tf3  [3]  [2]  *del_f3; 


.138 


fie [4]  = tfl  [4]  [2] *del_fl  + tf2 [4]  [2] *del_f2  + 

tf3  r 4 1 f 2 1 *del  f3 • 

fie [5]  = tfl [5] [2] *del_fl+tf2 [5] [2] *del_f2+ 

t f 3 [ 5 ] [2]*del_f3; 

printf ( " %f  %f  %f  %f  %f  %f \n" , fie [0] , fie [1] , 

f ie [2 ] , f ie [3 ] , f ie [4] ,fie[5] ) ; 
for  ( i=0  ; i<6  ; i++  ) 

f_tot[i]  = f_toto[i]  + fie[i]  ; 

printf ("Time  = %f\n  Estimated  = %f  %f  %f  %f  %f  %f\n", 

t+del_t , f_tot [ 0 ] , f_tot [ 1 ] , f_tot [ 2 ] , 

f_tot [ 3 ] , f_tot [ 4 ] , f_tot [ 5 ] ) ; 

/**  Start  Trial  move  to  prepare  for  the  next  time  **/ 


f _ext [ 0 ] = 
f _ext [ 1 ] = 
f_ext [2 ] = 
f _ext [ 3 ] = 
f_ext [ 4 ] = 
f _ext [ 5 ] = 


rcu [ 0 ] [ 0 ] * f_tot [ 0 ] +rcu [ 0 ] [ 1 ] * f_tot [ 1 ] + 

rcu [ 0 ] [2] * f tot [2]  ; 

rcu [ 1 ] [ 0 ] * f_tot [ 0 ] +rcu [ 1 ] [ 1 ] * f_tot [ 1 ] + 

rcu [ 1 ] [2] * f tot [2]  ; 

rcu[2] [0] *f_tot [0] +rcu[2] [ 1 ] * f_tot [ 1 ] + 

rcu [2]  [2] * f tot [2]  ; 

rcu[0] [0] *f_tot [3] +rcu[0] [1] *f_tot [4] + 

rcu [ 0] [2 ] *f_tot [ 5] ; 

rcu[l] [0] *f_tot [3] +rcu[l] [ 1 ] *f_tot [4 ] + 

rcu [ 1 ] [ 2 ] * f_tot [ 5 ] ; 

rcu [2]  [ 0 ] * f _tot [ 3 ] +rcu [ 2 ] [ 1 ] * f_tot [ 4 ] + 

rcu [2]  [ 2 ] * f_tot [ 5 ] ; 


printf ( "fx=%f  fy=%f  fz=%f  mx=%f  my=%f  mz=%f \n\n" , 
f _ext [ 0 ] , f _ext [ 1 ] , 

f _ext [2] , f _ext [ 3 ] , f _ext [ 4 ] , f _ext [ 5 ] ) ; 


check ( f _ext , f f , & f max ) ; 
fzstart  = fmax  ; 
fmin  = fmax  ; 
f zmax  = fmax ; 
tx  = ty  = tz  = 1.0; 
txc  = tyc  = tzc  = 0.0; 


/**  Search  For  the  Best  Direction  to  Rotate  using  sensed 

normal  force  ONLY  **/ 
rotate  ( rx,  1.0  , 'x'  ) ; 

mult 3 ( ro,rx,rcu); 
f_ext[0]  = ro [0] [0] *f_tot [0] + 

ro  [ 0 ] [ 1 ] * f _tot [ 1 ] +ro [ 0 ] [2]*f_tot [2] ; 

f_ext [ 1 ] = ro [ 1 ] [ 0 ] * f tot [ 0 ] + 

ro  [ 1 ] [1] *f_tot [1] +ro[l]  [2] *f_tot [2] ; 
f _ext [2]  = ro [2 ] [ 0 ] * f _tot [ 0 ] + 

ro [2 ] [1] *f_tot [1] +ro[2] [2] *f_tot [2] ; 
f_ext[3]  = ro[0] [ 0 ] * f_tot [ 3 ] + 


. 13  9 


ro [ 0 ] [ 1 ] * f _tot [ 4] +ro [ 0 ] [ 2 ] * f_tot [ 5 ] ; 
f_ext[4]  = ro [1] [0] *f_tot [3] + 

ro [ 1 ] [ 1 ] * f_tot [ 4 ] +ro [ 1 ] [ 2 ] * f_tot  [ 5 ] ; 
f _ext  [ 5 ] = ro [ 2 ] [ 0 ] * f_tot [ 3 ] + 

ro [2 ] [1] *f_tot [4] +ro [2 ] [2 ] *f_tot [5] ; 
check ( f _ext , f f , & f max ) ; 
if  ( fzstart  > fmax  ) 

{ fmin  = fmax; 


fminx  = fmax; 
txc  = 1.0; 


} 


x'  ) ; 


rotate  ( rl,  -1.0  , 
mult 3 ( ro,rl,rcu); 
f_ext[0]  = ro [0] [0] *f_tot [0] + 

ro [ 0 ] [ 1 ] * f_tot [ 1 ] +ro [ 0 ] [ 2 ] * f_tot [2] ; 
f_ext[l]  = ro  [ 1 ] [ 0 ] * f_tot  [ 0 ] + 

ro [ 1 ] [ 1 ] * f_tot [ 1 ] +ro [ 1 ] [ 2 ] * f_tot [ 2 ] ; 
f_ext[2]  = ro  [2]  [0] *f_tot  [0] + 

ro [ 2 ] [1] *f_tot [1] +ro[2] [2] *f_tot [2] ; 
f_ext[3]  = ro  [ 0 ] [ 0 ] * f_tot  [3 ] + 

ro [ 0 ] [ 1 ] * f_tot [4 ] +ro [ 0 ] [2 ] *f_tot [ 5 ] ; 
f_ext[4]  = ro  [1]  [0] *f_tot  [3]  + 

ro [1] [1] *f_tot [4] +ro [1] [2 ] *f_tot [5] ; 
f_ext[5]  = ro  [2]  [0] *f_tot  [3] + 

ro [2 ] [1] *f_tot [4] +ro[2] [2]*f_tot [5] ; 
check ( f_ext , f f , &fmax) ; 
if  ( txc  ==  1.0  &&  fmax  < fminx) 


txc  = -1.0; 
fmin  = fmax; 


fzstart  ) 


{ 

} 

else  if  ( txc  ==  0.0  &&  fmax 
{ txc  = -1.0; 

fmin  = fmax  ; 

} 

rotate  ( ry,  1.0  , 'y'  ); 

mult 3 ( ro,ry,rcu); 

f_ext[0]  = ro [0] [0] *f_tot [0] + 

ro [ 0 ] [ 1 ] * f_tot [ 1 ] +ro [ 0 ] 
f_ext[l]  = ro  [1]  [0] *f_tot  [0]  + 

ro [ 1 ] [ 1 ] * f_tot [ 1 ] +ro [ 1 ] 
f_ext [ 2 ] = ro  [2 ] [ 0 ] *f_tot [ 0 ] + 

ro [2 ] [ 1 ] *f_tot [ 1 ] +ro [ 2 ] 
f_ext[3]  = ro  [0]  [0] *f_tot  [3 ] + 

ro  [ 0 ] [ 1 ] * f_tot  [ 4 ] +ro  [ 0 ] 
f_ext[4]  = ro[l] [0] *f_tot [3] + 

ro[l] [1] *f_tot [4] +ro [ 1 ] 
f_ext[5]  = ro  [2 ] [ 0 ] * f_tot [3 ] + 

ro  [ 2 ] [ 1 ] * f_tot  [ 4 ] +ro  [ 2 ] 
check ( f_ext , f f , &fmax) ; 
if  ( fzstart  > fmax  ) 

{ fminy  = fmax; 
tyc  = 1.0; 


[2] *f_tot [2] ; 
[2 ] *f_tot [2] ; 


[2]  *f 
[2]  *f 
[2]*f. 
[2]*f 


tot [2 ] ; 
.tot  [ 5 ] ; 
tot [ 5 ] ; 
tot  [5]  ; 
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'y'  ) ; 


} 

if  ( fmin  > fmax  ) 
fmin  = fmax; 
rotate  ( r2 , -1.0  , 
mult 3 ( ro,r2,rcu); 
f_ext[0]  = ro [ 0 ] [ 0 ] * f_tot [ 0 ] + 

ro  [ 0 ] [ 1 ] * f_tot [ 1 ] +ro [ 0 ] [2]*f 
f_ext[l]  = ro [1] [0] *f_tot [0] + 

ro [ 1 ] [1] *f_tot [1] +ro[l] [2]*f 
f_ext[2]  = ro  [2]  [0]  *f_tot  [0]  + 

ro [2 ] [1] *f_tot [1] +ro[2] [2]*f 
f_ext[3]  = ro [0] [0] *f_tot [3] + 

ro [ 0 ] [1] *f_tot [4]+ro[0] [2]*f 
f_ext[4]  = ro [1] [ 0] *f_tot [3 ] + 

ro [ 1 ] [1] *f_tot [4] +ro[l] [2]*f 
f_ext[5]  = ro [2 ] [0 ] *f_tot [3 ] + 

ro [2 ] [ 1 ] *f_tot [4] +ro[2] [2]*f 
check ( f_ext , f f , &fmax) ; 
if  ( tyc  ==  1.0  &&  fmax  < fminy) 
tyc  = -1.0; 

else  if  ( tyc  ==  0.0  &&  fmax  < fzstart  ) 
tyc  = -1.0; 
if  ( fmin  > fmax  ) 
fmin  = fmax; 

rotate  ( rz,  1.0  , 'z'  ); 

mult 3 ( ro,rz,rcu) ; 

f_ext[0]  = ro [0] [0] *f_tot [0] + 

ro  [ 0 ] [ 1 ] * f_tot [ 1 ] +ro [ 0 ] [2]*f 
f_ext[l]  = ro [ 1 ] [ 0 ] * f_tot [ 0 ] + 

ro [1] [1] *f_tot [1] +ro[l] [2]*f 
f_ext[2]  = ro [2] [0] *f_tot [0] + 

ro [2 ] [1] *f_tot [l]+ro[2] [2]*f 
f_ext[3]  = ro [ 0 ] [ 0 ] *f_tot [3 ] + 

ro [0] [1] *f_tot [4] +ro[0] [2]*f 
f_ext[4]  = ro [1] [0] *f_tot [3 ] + 

ro  [ 1 ] [1]  *f_tot  [4-]  +ro[l]  [2]*f 
f_ext[5]  = ro [2 ] [ 0 ] *f_tot [3 ] + 

ro [2 ] [1] *f_tot [4] +ro[2] [2]*f 
check ( f _ext , f f , & fmax ) ; 
if  ( fzstart  > fmax  ) 

{ fminz  = fmax; 


_tot [2 ] ; 
_tot  [ 2 ] ; 

tot  [2 ] ; 

_tot  [ 5 ] ; 

tot [5] ; 

_tot  [ 5 ] ; 


tot  [ 2 ] ; 

tot  [2 ] ; 

__tot  [ 2 ] ; 

tot [ 5 ] ; 

tot [ 5 ] ; 

tot  [ 5 ] ; 


tzc  = 1.0; 

} 

if  ( fmin  > fmax  ) 
fmin  = fmax; 

rotate  ( rl,  -1.0  , 'z'  ); 

mult 3 ( ro,rl,rcu); 

f_ext[0]  = ro [0] [0] *f_tot [0] + 

ro [ 0 ] [1] * f t ot [1] +ro[0]  [2] *f_tot [2] ; 

f_ext[l]  = ro  [1]  [0] *f_tot [0]  + 

ro [ 1 ] [1] *f_tot [1] +ro[l] [2] *f_tot [2] ; 
f_ext[2]  = ro [2 ] [ 0 ] *f_tot [ 0 ] + 
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ro [ 2 ] [ 1 ] * f_tot [ 1 ] +ro [2]  [ 2 ] * f_tot [2] ; 
f_ext[3]  = ro  [0]  [ 0] *f_tot  [3  ] + 

ro [ 0 ] [ 1 ] * f_tot [ 4 ] +ro [ 0 ] [ 2 ] * f_tot [ 5 ] ; 
f _ext [ 4 ] = ro[l] [0] *f_tot [3]+ 

ro [ 1 ] [ 1 ] * f_tot [ 4 ] +ro [ 1 ] [ 2 ] * f_tot [ 5 ] ; 
f_ext[5]  = ro [2 ] [ 0 ] * f_tot [3 ] + 

ro [ 2 ] [ 1 ] * f _tot [ 4 ] +ro [ 2 ] [2]*  f _tot [ 5 ] ; 
check ( f_ext , f f , &fmax) ; 
if  ( tzc  ==  1.0  &&  fmax  < fminz) 
tzc  = -1.0; 

else  if  ( tzc  ==  0.0  &&  fmax  < fzstart  ) 
tzc  = -1.0; 
if  ( fmin  > fmax  ) 
fmin  = fmax; 

f tzc  = %f \n" , txc, tyc, tzc) ; 


printf ( " txc 

- % 

f tyc  = 

%f  tzc 

txo  = txc  * 

tx 

/180.0 

* 

pi; 

tyo  = tyc  * 

ty 

/ 180 . 0 

* 

pi; 

tzo  = tzc  * 

tz 

/180 . 0 

* 

pi ; 

printf ( " tx 

= %f 

ty  = % 

f 

tz  = 

/**  Choose  angles  with  the  axes  which  is  improving  grasp 


★ ★ 


/ 


if  ( fzmax  ==  fmin  ) 

{ print f ("Don't  Move\n" ) ; 
tx  = ty  = tz  = 0.0; 
txc  = tyc  = tzc  = 0.0; 
goto  cont ; 

} 

else 

{ 

tx  = txo ; 
ty  = tyo; 
tz  = tzo; 
count  = 1.0; 


keepgo : 


printf (" fzstart  = %f  \n" , fzstart  ); 

printf("tx  = %f  ty  = %f  tz  = 

%f \n" , tx/pi*180 , ty/pi*180 , tz/pi*180 ) ; 

ro [ 0 ] [ 0 ] = cos(tz)*cos(ty); 

ro [ 0 ] [1]  = cos(tz)*sin(ty)*sin(tx)-sin(tz) *cos ( tx)  ; 
ro [ 0 ] [2 ] = sin ( ty ) *cos ( tz ) *cos ( tx) +sin ( t z ) *sin ( tx) ; 
ro [ 1 ] [ 0 ] = sin ( tz ) *cos ( ty ) ; 

ro [ 1] [1 ] = cos ( tx) *cos ( tz ) +sin ( tx) *sin ( ty ) *sin ( tz ) ; 

ro [ 1 ] [2 ] = sin ( tz ) *sin ( ty ) *cos ( tx) -sin ( tx) *cos ( tz ) ; 

ro[2] [0]  = -sin(ty) ; 

ro [2 ] [ 1 ] = cos ( ty ) *sin ( tx) ; 

ro [2 ] [2 ] = cos ( tx) *cos ( ty ) ; 

mult 3 ( ro,ro,rcu  ); 

f_ext[0]  = ro[0] [ 0 ] * f_tot [ 0 ] + 

ro [ 0 ] [ 1 ] *f_tot [ 1 ] +ro [ 0 ] [2 ] *f_tot [2 ] ; 
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cont : 


f _ext [ 1 ] 


= ro[l] [ 0 ] * f_tot [ 0 ] + 

ro [1] [1] *f_tot [1] +ro[l] [2]*f_tot [2] ; 
= ro[2] [ 0 ] * f_tot [ 0 ] + 

ro [2 ] [1] *f_tot [1] +ro[2] [2] *f_tot [2] ; 
= ro[0] [ 0 ] *f_tot [ 3 ] + 

ro [ 0 ] [ 1 ] * f _tot [ 4 ] +ro [ 0 ] [ 2 ] * f _tot [ 5 ] ; 
= ro[l] [ 0 ] *f_tot [3 ] + 

ro [ 1 ] [ 1 ] * f_tot [ 4 ] +ro [ 1 ] [ 2 ] * f _tot [ 5 ] ; 
= ro  [2  ] [ 0 ] *f_tot [3 ] + 

ro  [2]  [1]  *f_tot  [4]  +ro  [2]  [2  ] *f_tot  [5]  ; 
check ( f_ext , f f , &fmax) ; 
if  ( fmax  < fzstart  ) 


f_ext [2 ] 
f _ext [ 3 ] 
f _ext [ 4 ] 
f_ext [ 5 ] 


{ 


} 

if  ( 
{ 


} 

if 

{ 


fzstart  = fmax  ; 
txc  = tx  ; 
tyc  = ty  ; 
tzc  = tz  ; 
count  +=1.0; 
tx  = txo  * count ; 
ty  = tyo  * count ; 
tz  = tzo  * count; 
goto  keepgo; 


) 


fmax  > fzstart  &&  count  > 1.0 
ro[0]‘[0]  = cos ( t zc ) *cos ( tyc ) ; 

= cos ( tzc ) *sin ( tyc ) *sin ( txc ) - 

sin (tzc) *cos (txc) ; 

= sin (tyc) *cos (tzc) *cos ( txc) + 

sin (tzc) *sin( txc ) ; 

= sin ( t zc ) *cos ( tyc ) ; 

= cos ( txc ) *cos ( tzc ) + 

sin (txc) *sin(tyc) *sin(tzc) ; 
= sin ( tzc ) *sin ( tyc ) *cos ( txc ) - 

sin ( txc ) *cos ( tzc ) ; 

= -sin ( tyc ) ; 

= cos ( tyc ) *sin ( txc ) ; 

= cos ( txc ) *cos ( tyc ) ; 


ro [ 0 ] [1] 

ro [ 0 ] [2] 

ro [ 1 ] [0] 
ro  [ 1 ] [ 1 ] 

ro [1]  [2] 

ro [ 2 ] [0] 
ro [2] [1] 
ro [ 2 ] [2] 


(fmax  > fzstart  &&  count  ==  1 ) 
for  ( i=0  ; i<3  ; i++  ) 

for  ( j = 0 ; j < 3 ; j ++ 
ro [ i ] [j]  =13  [i]  [j]  ; 
tx  = ty  = t z = 0.0; 
txc  = tyc  = tzc  = 0.0; 

} 

mult3  (rcu, ro, rcu) ; 
tx  = txc  *180. 0/pi; 
ty  = tyc  * 180. 0/pi; 
tz  = tzc  *180. 0/pi; 


if ( ( dabs ( f_tot [ 0 ] ) +dabs ( f_tot [ 1 ] ) + 

dabs ( f_tot  [2] ) ) ==0.0) 
f tot [ 1 ] = - 0.000000001; 
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f_ext[0]  = rcu[0] [ 0 ] *f_tot [ 0 ] + 

rcu [ 0 ] [ 1 ] * f_tot [ 1 ] +rcu [ 0 ] [ 2 ] * f_tot [2 ] ; 
f_ext[l]  = rcu[l] [ 0 ] *f_tot [ 0 ] + 

rcu [ 1 ] [ 1 ] * f_tot [ 1 ] + rcu [ 1 ] [ 2 ] * f_tot [ 2 ] ; 
f_ext[2]  = rcu [2] [ 0] *f_tot [0 ] + 

rcu [ 2 ] [ 1 ] * f_tot [ 1 ] +rcu [2]  [2 ] * f_tot [2]  ; 
f_ext[3]  = rcu[0] [0] *f_tot [3 ] + 

rcu [ 0 ] [ 1 ] * f_tot [ 4 ] +rcu [ 0 ] [ 2 ] * f_tot [ 5 ] ; 
f_ext[4]  = rcu [1] [0 ] *f_tot [3 ] + 

rcu [ 1 ] [ 1 ] * f_tot [ 4 ] +rcu [ 1 ] [ 2 ] * f _tot [ 5 ] ; 
f_ext[5]  = rcu [2 ] [ 0 ] * f_tot [ 3 ] + 

rcu [2 ] [ 1 ] * f_tot [4 ] +rcu [2]  [2 ] * f_tot [ 5 ] ; 
print f (" degree  ==>  x=%f  y=%f  z=%f \n" , tx, ty , tz ) ; 
check ( f_ext , f f , kfmax) ; 
if  ( f_sensed  ==  0 ) 
forg  = fmax; 
thx  +=  tx; 
thy  +=  ty; 
thz  +=  tz; 
print f ( "Time  = 


%f 


force  for  the  next 
time=%f \n " , t , fmax) ; 


} 


f print  f ( fp3  , " % f 


% 


% 


% 


% f 


} 


\n" , t , thx, thy, thz, f fmax, forg) ; 


step6 : 

print f ( "Nmax 

} 


= % f \ n " , Nmax ) ; 
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